From 6030298f43cfe744810b4ddf7162cfeace3b567b Mon Sep 17 00:00:00 2001
From: Andreas Reich <andreas.reich@unibw.de>
Date: Wed, 15 Dec 2021 14:49:28 +0100
Subject: [PATCH] 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.
---
 .../default_plugin/point_cloud_common.cpp     | 26 +++++++++++++++++++
 src/rviz/default_plugin/point_cloud_common.h  |  1 +
 2 files changed, 27 insertions(+)

diff --git a/src/rviz/default_plugin/point_cloud_common.cpp b/src/rviz/default_plugin/point_cloud_common.cpp
index 763b657f70..deca035cd3 100644
--- a/src/rviz/default_plugin/point_cloud_common.cpp
+++ b/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();
 }
 
diff --git a/src/rviz/default_plugin/point_cloud_common.h b/src/rviz/default_plugin/point_cloud_common.h
index 173a6b41f5..98eebdb125 100644
--- a/src/rviz/default_plugin/point_cloud_common.h
+++ b/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_;