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>(