Fixed crash on DepthCloudPlugin (#1133)

Signed-off-by: Alejandro Hernández Cordero <[email protected]>
ros2 · Feb 26, 2024 · 85bd663 · 85bd663
1 parent 82385de
commit 85bd663
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 1 deletion.
diff --git a/...default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp b/...default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp
@@ -104,6 +104,8 @@ protected Q_SLOTS:
   /** @brief Fill list of available and working transport options */
   void fillTransportOptionList(rviz_common::properties::EnumProperty * property);
 
+  void transformerChangedCallback();
+
   // Property callbacks
   virtual void updateTopic();
   virtual void updateTopicFilter();

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp
@@ -72,6 +72,7 @@ DepthCloudDisplay::DepthCloudDisplay()
 : rviz_common::Display()
   , messages_received_(0)
   , depthmap_sub_()
+  , depthmap_tf_filter_(nullptr)
   , rgb_sub_()
   , cam_info_sub_()
   , queue_size_(5)
@@ -161,6 +162,11 @@ DepthCloudDisplay::DepthCloudDisplay()
     use_occlusion_compensation_property_, SLOT(updateOcclusionTimeOut()), this);
 }
 
+void DepthCloudDisplay::transformerChangedCallback()
+{
+  updateTopic();
+}
+
 void DepthCloudDisplay::onInitialize()
 {
   auto rviz_ros_node_ = context_->getRosNodeAbstraction().lock();
@@ -184,6 +190,12 @@ void DepthCloudDisplay::onInitialize()
 
   depth_topic_property_->initialize(rviz_ros_node_);
   color_topic_property_->initialize(rviz_ros_node_);
+
+  QObject::connect(
+    reinterpret_cast<QObject *>(context_->getTransformationManager()),
+    SIGNAL(transformerChanged(std::shared_ptr<rviz_common::transformation::FrameTransformer>)),
+    this,
+    SLOT(transformerChangedCallback()));
 }
 
 DepthCloudDisplay::~DepthCloudDisplay()
@@ -209,6 +221,9 @@ void DepthCloudDisplay::setTopic(const QString & topic, const QString & datatype
 
 void DepthCloudDisplay::updateQueueSize()
 {
+  if (depthmap_tf_filter_) {
+    depthmap_tf_filter_->setQueueSize(static_cast<uint32_t>(queue_size_property_->getInt()));
+  }
   queue_size_ = queue_size_property_->getInt();
 }
 
@@ -303,7 +318,7 @@ void DepthCloudDisplay::subscribe()
           rviz_common::transformation::FrameTransformer>>(
         *context_->getFrameManager()->getTransformer(),
         fixed_frame_.toStdString(),
-        10,
+        queue_size_,
         rviz_ros_node_->get_raw_node());
 
       depthmap_tf_filter_->connectInput(*depthmap_sub_);
@@ -382,6 +397,9 @@ void DepthCloudDisplay::unsubscribe()
 void DepthCloudDisplay::clear()
 {
   pointcloud_common_->reset();
+  if (depthmap_tf_filter_) {
+    depthmap_tf_filter_->clear();
+  }
 }
 
 void DepthCloudDisplay::update(float wall_dt, float ros_dt)
@@ -603,6 +621,9 @@ void DepthCloudDisplay::fillTransportOptionList(rviz_common::properties::EnumPro
 
 void DepthCloudDisplay::fixedFrameChanged()
 {
+  if (depthmap_tf_filter_) {
+    depthmap_tf_filter_->setTargetFrame(fixed_frame_.toStdString());
+  }
   Display::reset();
 }