From 246aa58e4abf8bb982149a4ab8d579bab747d8b8 Mon Sep 17 00:00:00 2001
From: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com>
Date: Thu, 9 Jan 2025 09:29:06 +0100
Subject: [PATCH] fix: OctoMap and Filtered_Cloud Not Updating During Movement
 Execution (#3209)

* fix: adds MutuallyExclusive to pointcloud subscriber to allow octomap updates during motion

* refactor: adjust macro for readability
---
 .../pointcloud_octomap_updater.hpp                    |  1 +
 .../src/pointcloud_octomap_updater.cpp                | 11 ++++++++---
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp
index e768005753..cfe085e8c0 100644
--- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp
+++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp
@@ -37,6 +37,7 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/callback_group.hpp>
 #include <rclcpp/version.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/message_filter.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 74a6fcebae..31c294545d 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
@@ -114,13 +114,18 @@ 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_,
+  auto qos_profile =
 #if RCLCPP_VERSION_GTE(28, 3, 0)
-                                                                                           rclcpp::SensorDataQoS());
+      rclcpp::SensorDataQoS();
 #else
-                                                                                           rmw_qos_profile_sensor_data);
+      rmw_qos_profile_sensor_data;
 #endif
+  point_cloud_subscriber_ =
+      new message_filters::Subscriber<sensor_msgs::msg::PointCloud2>(node_, point_cloud_topic_, qos_profile, options);
   if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty())
   {
     point_cloud_filter_ = new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(