Remove tf subscription from frame manager and use tf buffer instead

ign_rviz_common/include/ignition/rviz/common/frame_manager.hpp

@@ -18,6 +18,7 @@
 #include <ignition/math/Pose3.hh>
 
 #include <QObject>
+#include <QTimer>
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -80,33 +81,27 @@ class FrameManager : public QObject
 
   /**
    * @brief Get available tf frames
-   * @param[out] _frames: List of available frames
+   * @return List of available frames
    */
-  void getFrames(std::vector<std::string> & _frames);
+  std::vector<std::string> getFrames();
 
   /**
    *  @brief Get fixed frame
    *  @return Fixed frame
    */
   std::string getFixedFrame();
 
-protected:
-  /**
-   * @brief Callback function to received transform messages
-   * @param[in] _msg: Transform message
-   */
-  void tf_callback(const tf2_msgs::msg::TFMessage::SharedPtr _msg);
+public slots:
+  void updateFrameList();
 
 private:
   rclcpp::Node::SharedPtr node;
   std::mutex tf_mutex_;
   std::string fixedFrame;
+  std::vector<std::string> frameList;
   std::shared_ptr<tf2_ros::Buffer> tfBuffer;
   std::shared_ptr<tf2_ros::TransformListener> tfListener;
-  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscriber;
-  std::unordered_map<std::string, ignition::math::Pose3d> tfTree;
-  tf2::TimePoint timePoint;
-  unsigned int frameCount;
+  QTimer frameListTimer;
 };
 }  // namespace common
 }  // namespace rviz

ign_rviz_common/src/rviz/common/frame_manager.cpp

@@ -35,24 +35,20 @@ namespace common
  * Creates a tf subscription and binds callback to it.
  */
 FrameManager::FrameManager(rclcpp::Node::SharedPtr _node)
-: QObject(), frameCount(0)
+: QObject(), node(std::move(_node))
 {
-  this->node = std::move(_node);
-
   tfBuffer = std::make_shared<tf2_ros::Buffer>(this->node->get_clock());
   tfListener = std::make_shared<tf2_ros::TransformListener>(*tfBuffer);
-
-  this->subscriber = this->node->create_subscription<tf2_msgs::msg::TFMessage>(
-    "/tf", 10,
-    std::bind(&FrameManager::tf_callback, this, std::placeholders::_1));
+  frameList = getFrames();
+  connect(&frameListTimer, SIGNAL(timeout()), this, SLOT(updateFrameList()));
+  frameListTimer.start(1000);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 void FrameManager::setFixedFrame(const std::string & _fixedFrame)
 {
   std::lock_guard<std::mutex>(this->tf_mutex_);
 
-  this->tfTree.clear();
   this->fixedFrame = _fixedFrame;
 
   // Send fixed frame changed event
@@ -71,102 +67,75 @@ std::string FrameManager::getFixedFrame()
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-void FrameManager::getFrames(std::vector<std::string> & _frames)
+std::vector<std::string> FrameManager::getFrames()
 {
-  tfBuffer->_getFrameStrings(_frames);
+  return tfBuffer->getAllFrameNames();
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-void FrameManager::tf_callback(const tf2_msgs::msg::TFMessage::SharedPtr _msg)
+bool FrameManager::getFramePose(const std::string & _frame, ignition::math::Pose3d & _pose)
 {
-  std::lock_guard<std::mutex>(this->tf_mutex_);
+  tf_mutex_.lock();
+  std::string fixedFrame = this->fixedFrame;
+  tf_mutex_.unlock();
 
-  if (this->fixedFrame.empty()) {
-    RCLCPP_ERROR(this->node->get_logger(), "No frame id specified");
-    return;
+  if (fixedFrame.empty()) {
+    RCLCPP_ERROR(this->node->get_logger(), "No fixed frame specified");
+    return false;
   }
 
-  std::vector<std::string> frame_ids;
-  tfBuffer->_getFrameStrings(frame_ids);
+  try {
+    geometry_msgs::msg::TransformStamped tf = tfBuffer->lookupTransform(
+      fixedFrame,
+      _frame, rclcpp::Time(0));
 
-  if (frame_ids.size() != this->frameCount) {
-    // Send frame list changed event
-    if (ignition::gui::App()) {
-      ignition::gui::App()->sendEvent(
-        ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
-        new events::FrameListChanged());
-    }
+    _pose = ignition::math::Pose3d(
+      tf.transform.translation.x,
+      tf.transform.translation.y,
+      tf.transform.translation.z,
+      tf.transform.rotation.w,
+      tf.transform.rotation.x,
+      tf.transform.rotation.y,
+      tf.transform.rotation.z);
 
-    this->frameCount = frame_ids.size();
+    return true;
+  } catch (tf2::TransformException & e) {
+    RCLCPP_WARN(this->node->get_logger(), e.what());
   }
 
-  builtin_interfaces::msg::Time timeStamp = _msg->transforms[0].header.stamp;
-  timePoint =
-    tf2::TimePoint(
-    std::chrono::seconds(timeStamp.sec) +
-    std::chrono::nanoseconds(timeStamp.nanosec));
-
-  for (const auto frame : frame_ids) {
-    try {
-      /*
-       * TODO(Sarathkrishnan Ramesh): Reducing the tiemout for lookupTransform affects
-       * smoothness of tf visualization.
-       */
-      geometry_msgs::msg::TransformStamped tf = tfBuffer->lookupTransform(
-        fixedFrame, timePoint,
-        frame, timePoint,
-        fixedFrame, tf2::Duration(5000));
-
-      tfTree[tf.child_frame_id] = ignition::math::Pose3d(
-        tf.transform.translation.x,
-        tf.transform.translation.y,
-        tf.transform.translation.z,
-        tf.transform.rotation.w,
-        tf.transform.rotation.x,
-        tf.transform.rotation.y,
-        tf.transform.rotation.z);
-    } catch (tf2::LookupException & e) {
-      RCLCPP_WARN(this->node->get_logger(), e.what());
-    } catch (tf2::ConnectivityException & e) {
-      RCLCPP_WARN(this->node->get_logger(), e.what());
-    } catch (tf2::ExtrapolationException & e) {
-      RCLCPP_WARN(this->node->get_logger(), e.what());
-    } catch (tf2::InvalidArgumentException & e) {
-      RCLCPP_WARN(this->node->get_logger(), e.what());
-    }
-  }
+  return false;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-bool FrameManager::getFramePose(const std::string & _frame, ignition::math::Pose3d & _pose)
+bool FrameManager::getParentPose(const std::string & _child, ignition::math::Pose3d & _pose)
 {
-  std::lock_guard<std::mutex>(this->tf_mutex_);
-
-  _pose = math::Pose3d::Zero;
-
-  auto it = this->tfTree.find(_frame);
-  if (it != tfTree.end()) {
-    _pose = it->second;
-    return true;
+  std::string parent;
+  // TODO(shrijitsingh99): The _getParent() API is only present for backwards compatability,
+  // use some alternative method instead
+  if (tfBuffer->_getParent(_child, tf2::TimePointZero, parent)) {
+    return this->getFramePose(parent, _pose);
   }
+
   return false;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-bool FrameManager::getParentPose(const std::string & _child, ignition::math::Pose3d & _pose)
+void FrameManager::updateFrameList()
 {
   std::lock_guard<std::mutex>(this->tf_mutex_);
-
-  std::string parent;
-  bool parentAvailable = tfBuffer->_getParent(_child, this->timePoint, parent);
-
-  if (!parentAvailable) {
-    return false;
+  auto updatedFrameList = getFrames();
+  if (frameList != updatedFrameList) {
+    // Send frame list changed event
+    if (ignition::gui::App()) {
+      ignition::gui::App()->sendEvent(
+        ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
+        new events::FrameListChanged());
+    }
   }
-
-  return this->getFramePose(parent, _pose);
+  frameList = std::move(updatedFrameList);
 }
 
+
 }  // namespace common
 }  // namespace rviz
 }  // namespace ignition

ign_rviz_plugins/include/ignition/rviz/plugins/AxesDisplay.hpp

@@ -104,7 +104,7 @@ public slots:
   /**
    * @brief Callback when refresh button is pressed.
    */
-  void onRefresh();
+  void refresh();
 
 private:
   void setScale();

ign_rviz_plugins/include/ignition/rviz/plugins/GlobalOptions.hpp

@@ -116,7 +116,7 @@ public slots:
   /**
    * @brief Callback when refresh button is pressed.
    */
-  void onRefresh();
+  void refresh();
 
 private:
   std::mutex lock;

ign_rviz_plugins/res/qml/AxesDisplay.qml

@@ -35,7 +35,7 @@ Item {
         text: "\u21bb"
         Material.background: Material.primary
         onClicked: {
-          AxesDisplay.onRefresh();
+          AxesDisplay.refresh();
         }
       }
 
@@ -51,6 +51,13 @@ Item {
 
           AxesDisplay.setFrame(textAt(currentIndex));
         }
+
+        Connections {
+          target: combo.popup
+          onAboutToShow: {
+            AxesDisplay.refresh();
+          }
+        }
       }
     }
 

ign_rviz_plugins/res/qml/GlobalOptions.qml

@@ -36,7 +36,7 @@ Item {
         text: "\u21bb"
         Material.background: Material.primary
         onClicked: {
-          GlobalOptions.onRefresh();
+          GlobalOptions.refresh();
         }
       }
 
@@ -56,6 +56,13 @@ Item {
           GlobalOptions.setFrame(textAt(currentIndex));
         }
 
+        Connections {
+          target: combo.popup
+          onAboutToShow: {
+            GlobalOptions.refresh();
+          }
+        }
+
         Connections {
           target: GlobalOptions
           onSetCurrentIndex: {

ign_rviz_plugins/src/rviz/plugins/AxesDisplay.cpp

@@ -87,9 +87,9 @@ bool AxesDisplay::eventFilter(QObject * object, QEvent * event)
   }
 
   // Update combo-box on frame list change
-  if (event->type() == rviz::events::FrameListChanged::kType) {
-    this->onRefresh();
-  }
+  // if (event->type() == rviz::events::FrameListChanged::kType) {
+  // this->onRefresh();
+  // }
 
   return QObject::eventFilter(object, event);
 }
@@ -148,7 +148,7 @@ void AxesDisplay::setFrameManager(std::shared_ptr<common::FrameManager> _frameMa
   this->frame = this->frameManager->getFixedFrame();
 
   // Update frame list
-  this->onRefresh();
+  this->refresh();
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -158,14 +158,13 @@ QStringList AxesDisplay::getFrameList() const
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-void AxesDisplay::onRefresh()
+void AxesDisplay::refresh()
 {
   // Clear
   this->frameList.clear();
 
   // Get updated list
-  std::vector<std::string> allFrames;
-  this->frameManager->getFrames(allFrames);
+  std::vector<std::string> allFrames = this->frameManager->getFrames();
   std::sort(allFrames.begin(), allFrames.end());
 
   this->frameList.push_back(QString::fromStdString("<Fixed Frame>"));

ign_rviz_plugins/src/rviz/plugins/GlobalOptions.cpp

@@ -69,7 +69,7 @@ void GlobalOptions::setFrameManager(std::shared_ptr<common::FrameManager> _frame
   this->frameList.clear();
 
   // Update frame list
-  this->onRefresh();
+  this->refresh();
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -99,11 +99,6 @@ bool GlobalOptions::eventFilter(QObject * _object, QEvent * _event)
     }
   }
 
-  // Update combo-box on frame list change
-  if (_event->type() == rviz::events::FrameListChanged::kType) {
-    this->onRefresh();
-  }
-
   return QObject::eventFilter(_object, _event);
 }
 
@@ -121,11 +116,10 @@ QStringList GlobalOptions::getFrameList() const
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-void GlobalOptions::onRefresh()
+void GlobalOptions::refresh()
 {
   // Get updated list
-  std::vector<std::string> allFrames;
-  this->frameManager->getFrames(allFrames);
+  std::vector<std::string> allFrames = this->frameManager->getFrames();
 
   if (allFrames.size() != 0) {
     // Clear

ign_rviz_plugins/src/rviz/plugins/TFDisplay.cpp

@@ -309,11 +309,10 @@ void TFDisplay::refresh()
 {
   std::lock_guard<std::mutex>(this->lock);
 
-  std::vector<std::string> frames;
-  this->frameManager->getFrames(frames);
+  std::vector<std::string> allFrames = this->frameManager->getFrames();
 
-  if (frames.size() > 0) {
-    for (const auto frame : frames) {
+  if (allFrames.size() > 0) {
+    for (const auto frame : allFrames) {
       this->frameInfo.insert({frame, true});
     }