fix: OctoMap and Filtered_Cloud Not Updating During Movement Executio…

…n (backport #3209) (#3211) (cherry picked from commit 246aa58) Co-authored-by: Marco Magri <[email protected]>
moveit · Jan 12, 2025 · 4ad2d10 · 4ad2d10
1 parent 55655af
commit 4ad2d10
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 2 deletions.
diff --git a/...ud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/...ud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h
@@ -37,6 +37,7 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/callback_group.hpp>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/message_filter.h>
 #include <message_filters/subscriber.h>

diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
@@ -118,9 +118,12 @@ void PointCloudOctomapUpdater::start()
 
   if (point_cloud_subscriber_)
     return;
+
+  rclcpp::SubscriptionOptions options;
+  options.callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   /* subscribe to point cloud topic using tf filter*/
-  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::msg::PointCloud2>(node_, point_cloud_topic_,
-                                                                                           rmw_qos_profile_sensor_data);
+  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::msg::PointCloud2>(
+      node_, point_cloud_topic_, rmw_qos_profile_sensor_data, options);
   if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty())
   {
     point_cloud_filter_ = new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(