point_cloud_common: add option to continuously transform the visuals …

…w.r.t. fixed frame

This option is particularly useful for messages, whose frame is moving w.r.t. fixed frame.

src/rviz/default_plugin/point_cloud_common.cpp

@@ -313,6 +313,12 @@ PointCloudCommon::PointCloudCommon(Display* display)
   , transformer_class_loader_(nullptr)
   , display_(display)
 {
+  continuous_transform_property_ =
+      new BoolProperty("Continuous Transform", false,
+                       "Retransform into fixed frame every timestep. This is particularly useful for "
+                       "messages whose frame moves w.r.t. fixed frame.",
+                       display_);
+
   selectable_property_ =
       new BoolProperty("Selectable", true,
                        "Whether or not the points in this point cloud are selectable.", display_,
@@ -637,6 +643,26 @@ void PointCloudCommon::update(float /*wall_dt*/, float /*ros_dt*/)
     new_color_transformer_ = false;
   }
 
+  if (continuous_transform_property_->getBool())
+  {
+    for (CloudInfoPtr& cloud_info : cloud_infos_)
+    {
+      if (!context_->getFrameManager()->getTransform(cloud_info->message_->header.frame_id, ros::Time(),
+                                                     cloud_info->position_, cloud_info->orientation_))
+      {
+        std::stringstream ss;
+        ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id
+           << "] to frame [" << context_->getFrameManager()->getFixedFrame() << "]";
+        display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
+      }
+      else
+      {
+        cloud_info->scene_node_->setPosition(cloud_info->position_);
+        cloud_info->scene_node_->setOrientation(cloud_info->orientation_);
+      }
+    }
+  }
+
   updateStatus();
 }
 

src/rviz/default_plugin/point_cloud_common.h

@@ -134,6 +134,7 @@ class PointCloudCommon : public QObject
 
   bool auto_size_;
 
+  BoolProperty* continuous_transform_property_;
   BoolProperty* selectable_property_;
   FloatProperty* point_world_size_property_;
   FloatProperty* point_pixel_size_property_;