Skip to content

Commit

Permalink
fix: OctoMap and Filtered_Cloud Not Updating During Movement Execution (
Browse files Browse the repository at this point in the history
#3209)

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

* refactor: adjust macro for readability
  • Loading branch information
MarcoMagriDev authored Jan 9, 2025
1 parent 75286c7 commit 246aa58
Show file tree
Hide file tree
Showing 2 changed files with 9 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>(
Expand Down

0 comments on commit 246aa58

Please sign in to comment.