Update Frame shortcut (ros-visualization#958) (ros-visualization#960)

* Update Frame shortcut Signed-off-by: David V. Lu <davidvlu@gmail.com> (cherry picked from commit 5eeae5b12e73b17f735ce62630b4ebf15c5f423e) Co-authored-by: David V. Lu!! <davidvlu@gmail.com>
130s · Jun 9, 2023 · c7fdd81 · c7fdd81
1 parent 801c11e
commit c7fdd81
Show file tree

Hide file tree

Showing 7 changed files with 29 additions and 29 deletions.
diff --git a/rviz_common/include/rviz_common/display.hpp b/rviz_common/include/rviz_common/display.hpp
@@ -285,6 +285,12 @@ class RVIZ_COMMON_PUBLIC Display : public rviz_common::properties::BoolProperty
   properties::Property *
   findProperty(const QString & name);
 
+  /// Get the latest transform to the frame and update the scene node. Return true on success.
+  bool updateFrame(const std::string & frame);
+
+  /// Get transform to the frame at the given time and update the scene node. True on success.
+  bool updateFrame(const std::string & frame, rclcpp::Time time);
+
 Q_SIGNALS:
   void
   timeSignal(rviz_common::Display * display, rclcpp::Time time);

diff --git a/rviz_common/src/rviz_common/display.cpp b/rviz_common/src/rviz_common/display.cpp
@@ -483,4 +483,22 @@ properties::Property * Display::findProperty(const QString & name)
   return nullptr;
 }
 
+
+bool Display::updateFrame(const std::string & frame)
+{
+  return updateFrame(frame, rclcpp::Time(0, 0, context_->getClock()->get_clock_type()));
+}
+
+bool Display::updateFrame(const std::string & frame, rclcpp::Time time)
+{
+  Ogre::Vector3 position;
+  Ogre::Quaternion orientation;
+  if (context_->getFrameManager()->getTransform(frame, time, position, orientation)) {
+    scene_node_->setPosition(position);
+    scene_node_->setOrientation(orientation);
+    return true;
+  }
+  return false;
+}
+
 }  // namespace rviz_common
diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp
@@ -151,11 +151,7 @@ void GridDisplay::update(float dt, float ros_dt)
   QString qframe = frame_property_->getFrame();
   std::string frame = qframe.toStdString();
 
-  Ogre::Vector3 position;
-  Ogre::Quaternion orientation;
-  if (context_->getFrameManager()->getTransform(frame, position, orientation)) {
-    scene_node_->setPosition(position);
-    scene_node_->setOrientation(orientation);
+  if (updateFrame(frame)) {
     setTransformOk();
     grid_->getSceneNode()->setVisible(true);
   } else {

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/grid_cells/grid_cells_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/grid_cells/grid_cells_display.cpp
@@ -131,16 +131,12 @@ void GridCellsDisplay::processMessage(nav_msgs::msg::GridCells::ConstSharedPtr m
 
 bool GridCellsDisplay::setTransform(std_msgs::msg::Header const & header)
 {
-  Ogre::Vector3 position;
-  Ogre::Quaternion orientation;
-  if (!context_->getFrameManager()->getTransform(header, position, orientation)) {
+  if (!updateFrame(header.frame_id, header.stamp)) {
     setMissingTransformToFixedFrame(header.frame_id, getNameStd());
     return false;
   }
   setTransformOk();
 
-  scene_node_->setPosition(position);
-  scene_node_->setOrientation(orientation);
   return true;
 }
 

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/point/point_stamped_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/point/point_stamped_display.cpp
@@ -128,11 +128,7 @@ void PointStampedDisplay::processMessage(geometry_msgs::msg::PointStamped::Const
     return;
   }
 
-  Ogre::Quaternion orientation;
-  Ogre::Vector3 position;
-  if (!context_->getFrameManager()->getTransform(
-      msg->header.frame_id, msg->header.stamp, position, orientation))
-  {
+  if (!updateFrame(msg->header.frame_id, msg->header.stamp)) {
     setMissingTransformToFixedFrame(msg->header.frame_id);
     return;
   }
@@ -142,9 +138,6 @@ void PointStampedDisplay::processMessage(geometry_msgs::msg::PointStamped::Const
     visuals_.pop_front();
   }
 
-  scene_node_->setPosition(position);
-  scene_node_->setOrientation(orientation);
-
   createNewSphereVisual(msg);
 }
 

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/polygon/polygon_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/polygon/polygon_display.cpp
@@ -104,17 +104,12 @@ void PolygonDisplay::processMessage(geometry_msgs::msg::PolygonStamped::ConstSha
     return;
   }
 
-  Ogre::Vector3 position;
-  Ogre::Quaternion orientation;
-  if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) {
+  if (!updateFrame(msg->header.frame_id, msg->header.stamp)) {
     setMissingTransformToFixedFrame(msg->header.frame_id);
     return;
   }
   setTransformOk();
 
-  scene_node_->setPosition(position);
-  scene_node_->setOrientation(orientation);
-
   manual_object_->clear();
 
   Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_property_->getColor());

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp
@@ -204,16 +204,12 @@ bool PoseArrayDisplay::validateFloats(const geometry_msgs::msg::PoseArray & msg)
 
 bool PoseArrayDisplay::setTransform(std_msgs::msg::Header const & header)
 {
-  Ogre::Vector3 position;
-  Ogre::Quaternion orientation;
-  if (!context_->getFrameManager()->getTransform(header, position, orientation)) {
+  if (!updateFrame(header.frame_id, header.stamp)) {
     setMissingTransformToFixedFrame(header.frame_id);
     return false;
   }
   setTransformOk();
 
-  scene_node_->setPosition(position);
-  scene_node_->setOrientation(orientation);
   return true;
 }