reduce amcl_node nodes (ros-navigation#2483)

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> add callback group and executor Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> update Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>
EnjoyRobotics · Jun 30, 2022 · 0002891 · 0002891
1 parent d9e1077
commit 0002891
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 11 deletions.
diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -105,6 +105,12 @@ class AmclNode : public nav2_util::LifecycleNode
   // respond until we're in the active state
   std::atomic<bool> active_{false};
 
+  // Dedicated callback group and executor for services and subscriptions in AmclNode,
+  // in order to isolate TF timer used in message filter.
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+
   // Pose hypothesis
   typedef struct
   {
@@ -165,7 +171,8 @@ class AmclNode : public nav2_util::LifecycleNode
    * @brief Initialize incoming data message subscribers and filters
    */
   void initMessageFilters();
-  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_;
+  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+    rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
   std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
   message_filters::Connection laser_scan_connection_;
 

diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
@@ -59,7 +59,7 @@ namespace nav2_amcl
 using nav2_util::geometry_utils::orientationAroundZAxis;
 
 AmclNode::AmclNode(const rclcpp::NodeOptions & options)
-: nav2_util::LifecycleNode("amcl", "", true, options)
+: nav2_util::LifecycleNode("amcl", "", false, options)
 {
   RCLCPP_INFO(get_logger(), "Creating");
 
@@ -236,7 +236,8 @@ nav2_util::CallbackReturn
 AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Configuring");
-
+  callback_group_ = create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
   initParameters();
   initTransforms();
   initParticleFilter();
@@ -245,7 +246,9 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
   initPubSub();
   initServices();
   initOdometry();
-
+  executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  executor_->add_callback_group(callback_group_, get_node_base_interface());
+  executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -317,6 +320,8 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Cleaning up");
 
+  executor_thread_.reset();
+
   // Get rid of the inputs first (services and message filter input), so we
   // don't continue to process incoming messages
   global_loc_srv_.reset();
@@ -1426,13 +1431,14 @@ AmclNode::initTransforms()
   RCLCPP_INFO(get_logger(), "initTransforms");
 
   // Initialize transform listener and broadcaster
-  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(),
+    callback_group_);
   tf_buffer_->setCreateTimerInterface(timer_interface);
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
-  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(rclcpp_node_);
+  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(shared_from_this(), true);
 
   sent_first_transform_ = false;
   latest_tf_valid_ = false;
@@ -1442,11 +1448,18 @@ AmclNode::initTransforms()
 void
 AmclNode::initMessageFilters()
 {
-  laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
-    rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
+  auto sub_opt = rclcpp::SubscriptionOptions();
+  sub_opt.callback_group = callback_group_;
+  laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+      rclcpp_lifecycle::LifecycleNode>>(
+    shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt);
 
   laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
-    *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_);
+    *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10,
+    get_node_logging_interface(),
+    get_node_clock_interface(),
+    transform_tolerance_);
+
 
   laser_scan_connection_ = laser_scan_filter_->registerCallback(
     std::bind(