reduce costmap2dros nodes

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

add multi threaded executor support for NodeThread

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

use multi threaded executor for costmap_2d_ros

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

use a separate callback group for range sensor layer

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

use a separate caallback group for static layer

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

use a separate callback group for costmap filter

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

nav2_controller/src/nav2_controller.cpp

@@ -63,7 +63,9 @@ ControllerServer::ControllerServer()
     "local_costmap", std::string{get_namespace()}, "local_costmap");
 
   // Launch a thread to run the costmap node
-  costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
+  auto costmap_executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  costmap_executor->add_node(costmap_ros_->get_node_base_interface());
+  costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_executor);
 }
 
 ControllerServer::~ControllerServer()

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -295,8 +295,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   bool getUseRadius() {return use_radius_;}
 
 protected:
-  rclcpp::Node::SharedPtr client_node_;
-
   // Publishers and subscribers
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
     footprint_pub_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp

@@ -100,6 +100,9 @@ class KeepoutFilter : public CostmapFilter
   rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;
 
+  // a separate callback group for filter and mask subscribers
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
   std::unique_ptr<Costmap2D> mask_costmap_;
 
   std::string mask_frame_;  // Frame where mask located in

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp

@@ -130,6 +130,9 @@ class SpeedFilter : public CostmapFilter
   rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;
 
+  // a separate callback group for filter and mask subscribers
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_pub_;
 
   nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -74,9 +74,7 @@ class Layer
     LayeredCostmap * parent,
     std::string name,
     tf2_ros::Buffer * tf,
-    const nav2_util::LifecycleNode::WeakPtr & node,
-    rclcpp::Node::SharedPtr client_node,
-    rclcpp::Node::SharedPtr rclcpp_node);
+    const nav2_util::LifecycleNode::WeakPtr & node);
   /** @brief Stop publishers. */
   virtual void deactivate() {}
   /** @brief Restart publishers if they've been stopped. */
@@ -160,8 +158,6 @@ class Layer
   LayeredCostmap * layered_costmap_;
   std::string name_;
   tf2_ros::Buffer * tf_;
-  rclcpp::Node::SharedPtr client_node_;
-  rclcpp::Node::SharedPtr rclcpp_node_;
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
   rclcpp::Clock::SharedPtr clock_;
   rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")};

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -224,7 +224,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<std::shared_ptr<message_filters::SubscriberBase<>>> observation_subscribers_;
+  std::vector<std::shared_ptr<message_filters::SubscriberBase<rclcpp_lifecycle::LifecycleNode>>>
+  observation_subscribers_;
   /// @brief Used to make sure that transforms are available for each sensor
   std::vector<std::shared_ptr<tf2_ros::MessageFilterBase>> observation_notifiers_;
   /// @brief Used to store observations from various sensors
@@ -234,6 +235,9 @@ class ObstacleLayer : public CostmapLayer
   /// @brief Used to store observation buffers used for clearing obstacles
   std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> clearing_buffers_;
 
+  //// @brief a separate callback group for observation message subscribers
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
   // Used only for testing purposes
   std::vector<nav2_costmap_2d::Observation> static_clearing_observations_;
   std::vector<nav2_costmap_2d::Observation> static_marking_observations_;

nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp

@@ -215,6 +215,8 @@ class RangeSensorLayer : public CostmapLayer
   rclcpp::Time last_reading_time_;
   unsigned int buffered_readings_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr> range_subs_;
+  // a separate callback group for range subscribers
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
   double min_x_, min_y_, max_x_, max_y_;
 
   /**

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -165,6 +165,9 @@ class StaticLayer : public CostmapLayer
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
   rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_update_sub_;
 
+  // a separate callback group for range subscribers
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
   // Parameters
   std::string map_topic_;
   bool map_subscribe_transient_local_;

nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

@@ -64,14 +64,20 @@ void KeepoutFilter::initializeFilter(
   }
 
   filter_info_topic_ = filter_info_topic;
+
+  // create a separate callback group for filter info and mask subscriber
+  callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto sub_opt = rclcpp::SubscriptionOptions();
+  sub_opt.callback_group = callback_group_;
+
   // Setting new costmap filter info subscriber
   RCLCPP_INFO(
     logger_,
     "KeepoutFilter: Subscribing to \"%s\" topic for filter info...",
     filter_info_topic_.c_str());
   filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
     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,14 +116,17 @@ 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_,
     "KeepoutFilter: Subscribing to \"%s\" topic for filter mask...",
     mask_topic_.c_str());
   mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
     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(

nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

@@ -72,14 +72,20 @@ void SpeedFilter::initializeFilter(
   node->get_parameter(name_ + "." + "speed_limit_topic", speed_limit_topic);
 
   filter_info_topic_ = filter_info_topic;
+
+  // create a separate callback group for filter info and mask subscriber
+  callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto sub_opt = rclcpp::SubscriptionOptions();
+  sub_opt.callback_group = callback_group_;
+
   // Setting new costmap filter info subscriber
   RCLCPP_INFO(
     logger_,
     "SpeedFilter: Subscribing to \"%s\" topic for filter info...",
     filter_info_topic_.c_str());
   filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
     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,14 +149,17 @@ 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_,
     "SpeedFilter: Subscribing to \"%s\" topic for filter mask...",
     mask_topic_.c_str());
   mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
     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(

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -112,6 +112,11 @@ void ObstacleLayer::onInitialize()
 
   global_frame_ = layered_costmap_->getGlobalFrameID();
 
+  // create a separate callback group for observation message subscriber
+  callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  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);
 
@@ -211,14 +216,14 @@ void ObstacleLayer::onInitialize()
 
     // create a callback for the topic
     if (data_type == "LaserScan") {
-      std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> sub(
-        new message_filters::Subscriber<sensor_msgs::msg::LaserScan>(
-          rclcpp_node_, topic, custom_qos_profile));
+      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+          rclcpp_lifecycle::LifecycleNode>>(node, topic, custom_qos_profile, sub_opt);
       sub->unsubscribe();
 
-      std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> filter(
-        new tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>(
-          *sub, *tf_, global_frame_, 50, rclcpp_node_));
+      auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
+        *sub, *tf_, global_frame_, 50,
+        node->get_node_logging_interface(),
+        node->get_node_clock_interface());
 
       if (inf_is_valid) {
         filter->registerCallback(
@@ -239,9 +244,8 @@ void ObstacleLayer::onInitialize()
       observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05));
 
     } else {
-      std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> sub(
-        new message_filters::Subscriber<sensor_msgs::msg::PointCloud2>(
-          rclcpp_node_, topic, custom_qos_profile));
+      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
+          rclcpp_lifecycle::LifecycleNode>>(node, topic, custom_qos_profile, sub_opt);
       sub->unsubscribe();
 
       if (inf_is_valid) {
@@ -250,9 +254,10 @@ void ObstacleLayer::onInitialize()
           "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
       }
 
-      std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> filter(
-        new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(
-          *sub, *tf_, global_frame_, 50, rclcpp_node_));
+      auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>>(
+        *sub, *tf_, global_frame_, 50,
+        node->get_node_logging_interface(),
+        node->get_node_clock_interface());
 
       filter->registerCallback(
         std::bind(

nav2_costmap_2d/plugins/range_sensor_layer.cpp

@@ -128,6 +128,11 @@ void RangeSensorLayer::onInitialize()
     return;
   }
 
+  // create a separate callback group for range subscriber
+  callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  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 +158,7 @@ void RangeSensorLayer::onInitialize()
       node->create_subscription<sensor_msgs::msg::Range>(
         topic_name, rclcpp::SensorDataQoS(), std::bind(
           &RangeSensorLayer::bufferIncomingRangeMsg, this,
-          std::placeholders::_1)));
+          std::placeholders::_1), sub_opt));
 
     RCLCPP_INFO(
       logger_, "RangeSensorLayer: subscribed to "

nav2_costmap_2d/plugins/static_layer.cpp

@@ -89,16 +89,21 @@ StaticLayer::onInitialize()
     throw std::runtime_error{"Failed to lock node"};
   }
 
+  // create a separate callback group for map subscriber
+  callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto sub_opt = rclcpp::SubscriptionOptions();
+  sub_opt.callback_group = callback_group_;
+
   map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
     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_msgs::msg::OccupancyGridUpdate>(
       map_topic_ + "_updates",
       rclcpp::SystemDefaultsQoS(),
-      std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
+      std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1), sub_opt);
   }
 }
 

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -62,7 +62,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
@@ -81,9 +81,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<rclcpp::Node>("_", options);
 
   std::vector<std::string> clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"};
 
@@ -137,10 +134,10 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   }
 
   // Create the transform-related objects
-  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(rclcpp_node_->get_clock());
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
   auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
-    rclcpp_node_->get_node_base_interface(),
-    rclcpp_node_->get_node_timers_interface());
+    get_node_base_interface(),
+    get_node_timers_interface());
   tf_buffer_->setCreateTimerInterface(timer_interface);
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
@@ -153,8 +150,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_);
+      layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), shared_from_this());
 
     RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str());
   }
@@ -166,8 +162,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
     layered_costmap_->addFilter(filter);
 
     filter->initialize(
-      layered_costmap_.get(), filter_names_[i], tf_buffer_.get(),
-      shared_from_this(), client_node_, rclcpp_node_);
+      layered_costmap_.get(), filter_names_[i], tf_buffer_.get(), shared_from_this());
 
     RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str());
   }

nav2_costmap_2d/src/layer.cpp

@@ -49,17 +49,12 @@ Layer::initialize(
   LayeredCostmap * parent,
   std::string name,
   tf2_ros::Buffer * tf,
-  const nav2_util::LifecycleNode::WeakPtr & node,
-  rclcpp::Node::SharedPtr client_node,
-  rclcpp::Node::SharedPtr rclcpp_node)
+  const nav2_util::LifecycleNode::WeakPtr & node)
 {
   layered_costmap_ = parent;
   name_ = name;
   tf_ = tf;
-  client_node_ = client_node;
-  rclcpp_node_ = rclcpp_node;
   node_ = node;
-
   {
     auto node_shared_ptr = node_.lock();
     logger_ = node_shared_ptr->get_logger();

nav2_costmap_2d/test/testing_helper.hpp

@@ -75,7 +75,7 @@ void addStaticLayer(
 {
   slayer = std::make_shared<nav2_costmap_2d::StaticLayer>();
   layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(slayer));
-  slayer->initialize(&layers, "static", &tf, node, nullptr, nullptr /*TODO*/);
+  slayer->initialize(&layers, "static", &tf, node);
 }
 
 void addObstacleLayer(
@@ -84,7 +84,7 @@ void addObstacleLayer(
   std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer)
 {
   olayer = std::make_shared<nav2_costmap_2d::ObstacleLayer>();
-  olayer->initialize(&layers, "obstacles", &tf, node, nullptr, nullptr /*TODO*/);
+  olayer->initialize(&layers, "obstacles", &tf, node);
   layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(olayer));
 }
 
@@ -94,7 +94,7 @@ void addRangeLayer(
   std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> & rlayer)
 {
   rlayer = std::make_shared<nav2_costmap_2d::RangeSensorLayer>();
-  rlayer->initialize(&layers, "range", &tf, node, nullptr, nullptr /*TODO*/);
+  rlayer->initialize(&layers, "range", &tf, node);
   layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(rlayer));
 }
 
@@ -133,7 +133,7 @@ void addInflationLayer(
   std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer)
 {
   ilayer = std::make_shared<nav2_costmap_2d::InflationLayer>();
-  ilayer->initialize(&layers, "inflation", &tf, node, nullptr, nullptr /*TODO*/);
+  ilayer->initialize(&layers, "inflation", &tf, node);
   std::shared_ptr<nav2_costmap_2d::Layer> ipointer(ilayer);
   layers.addPlugin(ipointer);
 }