From 972c433e63a26900deb07c6022c50628f33b770b Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Fri, 27 May 2022 19:32:00 +0800 Subject: [PATCH 1/2] use a dedicated callback group for costmap plugin and filter instead of inner node Signed-off-by: zhenpeng ge update comment Signed-off-by: zhenpeng ge --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 7 +++-- .../include/nav2_costmap_2d/layer.hpp | 6 ++-- .../nav2_costmap_2d/obstacle_layer.hpp | 3 +- .../costmap_filters/keepout_filter.cpp | 11 +++++-- .../plugins/costmap_filters/speed_filter.cpp | 11 +++++-- nav2_costmap_2d/plugins/obstacle_layer.cpp | 29 +++++++++++-------- .../plugins/range_sensor_layer.cpp | 5 +++- nav2_costmap_2d/plugins/static_layer.cpp | 7 +++-- nav2_costmap_2d/src/costmap_2d_ros.cpp | 23 +++++++++------ nav2_costmap_2d/src/layer.cpp | 7 ++--- nav2_costmap_2d/test/testing_helper.hpp | 8 ++--- .../test/unit/declare_parameter_test.cpp | 4 +-- .../test/unit/keepout_filter_test.cpp | 4 +-- .../test/unit/speed_filter_test.cpp | 2 +- 14 files changed, 77 insertions(+), 50 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index c9ae8044b0..fc152ccb95 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -303,8 +303,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode double getRobotRadius() {return robot_radius_;} protected: - rclcpp::Node::SharedPtr client_node_; - // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; @@ -313,6 +311,11 @@ class Costmap2DROS : public nav2_util::LifecycleNode rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; + // Dedicated callback group and executor for tf timer_interface, costmap plugins and filters + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + // Transform listener std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 5f8c152dc4..72125143ba 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -75,8 +75,7 @@ class Layer std::string name, tf2_ros::Buffer * tf, const nav2_util::LifecycleNode::WeakPtr & node, - rclcpp::Node::SharedPtr client_node, - rclcpp::Node::SharedPtr rclcpp_node); + rclcpp::CallbackGroup::SharedPtr callback_group); /** @brief Stop publishers. */ virtual void deactivate() {} /** @brief Restart publishers if they've been stopped. */ @@ -160,8 +159,7 @@ class Layer LayeredCostmap * layered_costmap_; std::string name_; tf2_ros::Buffer * tf_; - rclcpp::Node::SharedPtr client_node_; - rclcpp::Node::SharedPtr rclcpp_node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp_lifecycle::LifecycleNode::WeakPtr node_; rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 66558b26d2..fbced80c80 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -231,7 +231,8 @@ class ObstacleLayer : public CostmapLayer /// @brief Used to project laser scans into point clouds laser_geometry::LaserProjection projector_; /// @brief Used for the observation message filters - std::vector>> observation_subscribers_; + std::vector>> + observation_subscribers_; /// @brief Used to make sure that transforms are available for each sensor std::vector> observation_notifiers_; /// @brief Used to store observations from various sensors diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 4a5571e42e..96729aa66a 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -64,6 +64,10 @@ void KeepoutFilter::initializeFilter( } filter_info_topic_ = filter_info_topic; + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + // Setting new costmap filter info subscriber RCLCPP_INFO( logger_, @@ -71,7 +75,7 @@ void KeepoutFilter::initializeFilter( filter_info_topic_.c_str()); filter_info_sub_ = node->create_subscription( filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); + std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1), sub_opt); global_frame_ = layered_costmap_->getGlobalFrameID(); } @@ -110,6 +114,9 @@ void KeepoutFilter::filterInfoCallback( mask_topic_ = msg->filter_mask_topic; + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + // Setting new filter mask subscriber RCLCPP_INFO( logger_, @@ -117,7 +124,7 @@ void KeepoutFilter::filterInfoCallback( mask_topic_.c_str()); mask_sub_ = node->create_subscription( mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1)); + std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1), sub_opt); } void KeepoutFilter::maskCallback( diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index cc368b4683..f1691c331a 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -72,6 +72,10 @@ void SpeedFilter::initializeFilter( node->get_parameter(name_ + "." + "speed_limit_topic", speed_limit_topic); filter_info_topic_ = filter_info_topic; + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + // Setting new costmap filter info subscriber RCLCPP_INFO( logger_, @@ -79,7 +83,7 @@ void SpeedFilter::initializeFilter( filter_info_topic_.c_str()); filter_info_sub_ = node->create_subscription( filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&SpeedFilter::filterInfoCallback, this, std::placeholders::_1)); + std::bind(&SpeedFilter::filterInfoCallback, this, std::placeholders::_1), sub_opt); // Get global frame required for speed limit publisher global_frame_ = layered_costmap_->getGlobalFrameID(); @@ -143,6 +147,9 @@ void SpeedFilter::filterInfoCallback( mask_topic_ = msg->filter_mask_topic; + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + // Setting new filter mask subscriber RCLCPP_INFO( logger_, @@ -150,7 +157,7 @@ void SpeedFilter::filterInfoCallback( mask_topic_.c_str()); mask_sub_ = node->create_subscription( mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1)); + std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1), sub_opt); } void SpeedFilter::maskCallback( diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index f4883a2b13..e7836f9639 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -119,6 +119,9 @@ void ObstacleLayer::onInitialize() global_frame_ = layered_costmap_->getGlobalFrameID(); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + // now we need to split the topics based on whitespace which we can use a stringstream for std::stringstream ss(topics_string); @@ -218,14 +221,15 @@ void ObstacleLayer::onInitialize() // create a callback for the topic if (data_type == "LaserScan") { - std::shared_ptr> sub( - new message_filters::Subscriber( - rclcpp_node_, topic, custom_qos_profile)); + auto sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); sub->unsubscribe(); - std::shared_ptr> filter( - new tf2_ros::MessageFilter( - *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance))); + auto filter = std::make_shared>( + *sub, *tf_, global_frame_, 50, + node->get_node_logging_interface(), + node->get_node_clock_interface(), + tf2::durationFromSec(transform_tolerance)); if (inf_is_valid) { filter->registerCallback( @@ -246,9 +250,8 @@ void ObstacleLayer::onInitialize() observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); } else { - std::shared_ptr> sub( - new message_filters::Subscriber( - rclcpp_node_, topic, custom_qos_profile)); + auto sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); sub->unsubscribe(); if (inf_is_valid) { @@ -257,9 +260,11 @@ void ObstacleLayer::onInitialize() "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); } - std::shared_ptr> filter( - new tf2_ros::MessageFilter( - *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance))); + auto filter = std::make_shared>( + *sub, *tf_, global_frame_, 50, + node->get_node_logging_interface(), + node->get_node_clock_interface(), + tf2::durationFromSec(transform_tolerance)); filter->registerCallback( std::bind( diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index c8020781d6..91f48dc440 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -128,6 +128,9 @@ void RangeSensorLayer::onInitialize() return; } + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + // Traverse the topic names list subscribing to all of them with the same callback method for (auto & topic_name : topic_names) { if (input_sensor_type == InputSensorType::VARIABLE) { @@ -153,7 +156,7 @@ void RangeSensorLayer::onInitialize() node->create_subscription( topic_name, rclcpp::SensorDataQoS(), std::bind( &RangeSensorLayer::bufferIncomingRangeMsg, this, - std::placeholders::_1))); + std::placeholders::_1), sub_opt)); RCLCPP_INFO( logger_, "RangeSensorLayer: subscribed to " diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 073109c235..d574e666fb 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -90,16 +90,19 @@ StaticLayer::onInitialize() throw std::runtime_error{"Failed to lock node"}; } + // use a dedicated callback group for map and map_update subscriber + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; map_sub_ = node->create_subscription( map_topic_, map_qos, - std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1)); + std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1), sub_opt); if (subscribe_to_updates_) { RCLCPP_INFO(logger_, "Subscribing to updates"); map_update_sub_ = node->create_subscription( map_topic_ + "_updates", rclcpp::SystemDefaultsQoS(), - std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1)); + std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1), sub_opt); } } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 58342a8ad8..437df3eace 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -65,7 +65,7 @@ Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, const std::string & local_namespace) -: nav2_util::LifecycleNode(name, "", true, +: nav2_util::LifecycleNode(name, "", false, // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace // TODO(orduno) Pass a sub-node instead of creating a new node for better handling @@ -84,9 +84,6 @@ Costmap2DROS::Costmap2DROS( "nav2_costmap_2d::InflationLayer"} { RCLCPP_INFO(get_logger(), "Creating Costmap"); - auto options = rclcpp::NodeOptions().arguments( - {"--ros-args", "-r", std::string("__node:=") + get_name() + "_client", "--"}); - client_node_ = std::make_shared("_", options); std::vector clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"}; @@ -129,6 +126,9 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Configuring"); getParameters(); + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + // Create the costmap itself layered_costmap_ = std::make_unique( global_frame_, rolling_window_, track_unknown_space_); @@ -140,10 +140,11 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) } // Create the transform-related objects - tf_buffer_ = std::make_shared(rclcpp_node_->get_clock()); + tf_buffer_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( - rclcpp_node_->get_node_base_interface(), - rclcpp_node_->get_node_timers_interface()); + get_node_base_interface(), + get_node_timers_interface(), + callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); @@ -157,7 +158,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // TODO(mjeronimo): instead of get(), use a shared ptr plugin->initialize( layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), - shared_from_this(), client_node_, rclcpp_node_); + shared_from_this(), callback_group_); RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); } @@ -170,7 +171,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) filter->initialize( layered_costmap_.get(), filter_names_[i], tf_buffer_.get(), - shared_from_this(), client_node_, rclcpp_node_); + shared_from_this(), callback_group_); RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str()); } @@ -201,6 +202,9 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // Add cleaning service clear_costmap_service_ = std::make_unique(shared_from_this(), *this); + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); return nav2_util::CallbackReturn::SUCCESS; } @@ -284,6 +288,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) costmap_publisher_.reset(); clear_costmap_service_.reset(); + executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index e76f7de7bf..852b4b29b5 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -50,16 +50,13 @@ Layer::initialize( std::string name, tf2_ros::Buffer * tf, const nav2_util::LifecycleNode::WeakPtr & node, - rclcpp::Node::SharedPtr client_node, - rclcpp::Node::SharedPtr rclcpp_node) + rclcpp::CallbackGroup::SharedPtr callback_group) { layered_costmap_ = parent; name_ = name; tf_ = tf; - client_node_ = client_node; - rclcpp_node_ = rclcpp_node; node_ = node; - + callback_group_ = callback_group; { auto node_shared_ptr = node_.lock(); logger_ = node_shared_ptr->get_logger(); diff --git a/nav2_costmap_2d/test/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp index 84aa0e4d83..a24b166a73 100644 --- a/nav2_costmap_2d/test/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -75,7 +75,7 @@ void addStaticLayer( { slayer = std::make_shared(); layers.addPlugin(std::shared_ptr(slayer)); - slayer->initialize(&layers, "static", &tf, node, nullptr, nullptr /*TODO*/); + slayer->initialize(&layers, "static", &tf, node, nullptr); } void addObstacleLayer( @@ -84,7 +84,7 @@ void addObstacleLayer( std::shared_ptr & olayer) { olayer = std::make_shared(); - olayer->initialize(&layers, "obstacles", &tf, node, nullptr, nullptr /*TODO*/); + olayer->initialize(&layers, "obstacles", &tf, node, nullptr); layers.addPlugin(std::shared_ptr(olayer)); } @@ -94,7 +94,7 @@ void addRangeLayer( std::shared_ptr & rlayer) { rlayer = std::make_shared(); - rlayer->initialize(&layers, "range", &tf, node, nullptr, nullptr /*TODO*/); + rlayer->initialize(&layers, "range", &tf, node, nullptr); layers.addPlugin(std::shared_ptr(rlayer)); } @@ -133,7 +133,7 @@ void addInflationLayer( std::shared_ptr & ilayer) { ilayer = std::make_shared(); - ilayer->initialize(&layers, "inflation", &tf, node, nullptr, nullptr /*TODO*/); + ilayer->initialize(&layers, "inflation", &tf, node, nullptr); std::shared_ptr ipointer(ilayer); layers.addPlugin(ipointer); } diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp index 4edb6af29c..a8a717c8f9 100644 --- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -44,7 +44,7 @@ TEST(DeclareParameter, useValidParameter) tf2_ros::Buffer tf(node->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); - layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + layer.initialize(&layers, "test_layer", &tf, node, nullptr); layer.declareParameter("test1", rclcpp::ParameterValue("test_val1")); try { @@ -63,7 +63,7 @@ TEST(DeclareParameter, useInvalidParameter) tf2_ros::Buffer tf(node->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); - layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + layer.initialize(&layers, "test_layer", &tf, node, nullptr); layer.declareParameter("test2", rclcpp::PARAMETER_STRING); try { diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index 2607108707..304b423f5c 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -225,9 +225,7 @@ void TestNode::createKeepoutFilter(const std::string & global_frame) rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); keepout_filter_ = std::make_shared(); - keepout_filter_->initialize( - &layers, std::string(FILTER_NAME), - tf_buffer_.get(), node_, nullptr, nullptr); + keepout_filter_->initialize(&layers, std::string(FILTER_NAME), tf_buffer_.get(), node_, nullptr); keepout_filter_->initializeFilter(INFO_TOPIC); // Wait until mask will be received by KeepoutFilter diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index a7dda6093d..d419cade36 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -362,7 +362,7 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) rclcpp::Parameter(std::string(FILTER_NAME) + ".speed_limit_topic", SPEED_LIMIT_TOPIC)); speed_filter_ = std::make_shared(); - speed_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr, nullptr); + speed_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr); speed_filter_->initializeFilter(INFO_TOPIC); speed_limit_subscriber_ = std::make_shared(SPEED_LIMIT_TOPIC); From d7ab41c9e29c72e3d3aca3fd03f684d02732714b Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Thu, 2 Jun 2022 13:13:25 +0800 Subject: [PATCH 2/2] remove unnecessary modifications Signed-off-by: zhenpeng ge --- .../plugins/costmap_filters/keepout_filter.cpp | 11 ++--------- .../plugins/costmap_filters/speed_filter.cpp | 11 ++--------- nav2_costmap_2d/plugins/range_sensor_layer.cpp | 5 +---- nav2_costmap_2d/plugins/static_layer.cpp | 7 ++----- 4 files changed, 7 insertions(+), 27 deletions(-) diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 96729aa66a..4a5571e42e 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -64,10 +64,6 @@ void KeepoutFilter::initializeFilter( } filter_info_topic_ = filter_info_topic; - - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group_; - // Setting new costmap filter info subscriber RCLCPP_INFO( logger_, @@ -75,7 +71,7 @@ void KeepoutFilter::initializeFilter( filter_info_topic_.c_str()); filter_info_sub_ = node->create_subscription( filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1), sub_opt); + std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); global_frame_ = layered_costmap_->getGlobalFrameID(); } @@ -114,9 +110,6 @@ void KeepoutFilter::filterInfoCallback( mask_topic_ = msg->filter_mask_topic; - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group_; - // Setting new filter mask subscriber RCLCPP_INFO( logger_, @@ -124,7 +117,7 @@ void KeepoutFilter::filterInfoCallback( mask_topic_.c_str()); mask_sub_ = node->create_subscription( mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1), sub_opt); + std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1)); } void KeepoutFilter::maskCallback( diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index f1691c331a..cc368b4683 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -72,10 +72,6 @@ void SpeedFilter::initializeFilter( node->get_parameter(name_ + "." + "speed_limit_topic", speed_limit_topic); filter_info_topic_ = filter_info_topic; - - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group_; - // Setting new costmap filter info subscriber RCLCPP_INFO( logger_, @@ -83,7 +79,7 @@ void SpeedFilter::initializeFilter( filter_info_topic_.c_str()); filter_info_sub_ = node->create_subscription( filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&SpeedFilter::filterInfoCallback, this, std::placeholders::_1), sub_opt); + std::bind(&SpeedFilter::filterInfoCallback, this, std::placeholders::_1)); // Get global frame required for speed limit publisher global_frame_ = layered_costmap_->getGlobalFrameID(); @@ -147,9 +143,6 @@ void SpeedFilter::filterInfoCallback( mask_topic_ = msg->filter_mask_topic; - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group_; - // Setting new filter mask subscriber RCLCPP_INFO( logger_, @@ -157,7 +150,7 @@ void SpeedFilter::filterInfoCallback( mask_topic_.c_str()); mask_sub_ = node->create_subscription( mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1), sub_opt); + std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1)); } void SpeedFilter::maskCallback( diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index 91f48dc440..c8020781d6 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -128,9 +128,6 @@ void RangeSensorLayer::onInitialize() return; } - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group_; - // Traverse the topic names list subscribing to all of them with the same callback method for (auto & topic_name : topic_names) { if (input_sensor_type == InputSensorType::VARIABLE) { @@ -156,7 +153,7 @@ void RangeSensorLayer::onInitialize() node->create_subscription( topic_name, rclcpp::SensorDataQoS(), std::bind( &RangeSensorLayer::bufferIncomingRangeMsg, this, - std::placeholders::_1), sub_opt)); + std::placeholders::_1))); RCLCPP_INFO( logger_, "RangeSensorLayer: subscribed to " diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index d574e666fb..073109c235 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -90,19 +90,16 @@ StaticLayer::onInitialize() throw std::runtime_error{"Failed to lock node"}; } - // use a dedicated callback group for map and map_update subscriber - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group_; map_sub_ = node->create_subscription( map_topic_, map_qos, - std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1), sub_opt); + std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1)); if (subscribe_to_updates_) { RCLCPP_INFO(logger_, "Subscribing to updates"); map_update_sub_ = node->create_subscription( map_topic_ + "_updates", rclcpp::SystemDefaultsQoS(), - std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1), sub_opt); + std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1)); } }