From 4ad2d10f4a5bfa95b29d4a7804317c8233b47cc6 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Sun, 12 Jan 2025 17:32:14 -0500
Subject: [PATCH] fix: OctoMap and Filtered_Cloud Not Updating During Movement
 Execution (backport #3209) (#3211)

(cherry picked from commit 246aa58e4abf8bb982149a4ab8d579bab747d8b8)

Co-authored-by: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com>
---
 .../pointcloud_octomap_updater.h                           | 1 +
 .../src/pointcloud_octomap_updater.cpp                     | 7 +++++--
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h
index b38fbf7530..4b4b3615d5 100644
--- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h
+++ b/moveit_ros/perception/pointcloud_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
index 5a56215a3b..4e1e1aa266 100644
--- 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>(