Reduce nodes for nav2_waypoint_follower (#2441)

* Reduce node of waypoint_follower by using callback group and executor Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * fix linting failures Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>
ros-navigation · Jul 13, 2021 · eaeb3e0 · eaeb3e0
1 parent f4b097e
commit eaeb3e0
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 12 deletions.
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
@@ -118,7 +118,8 @@ class WaypointFollower : public nav2_util::LifecycleNode
   // Our action server
   std::unique_ptr<ActionServer> action_server_;
   ActionClient::SharedPtr nav_to_pose_client_;
-  rclcpp::Node::SharedPtr client_node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
   std::shared_future<rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr> future_goal_handle_;
   bool stop_on_failure_;
   ActionStatus current_goal_status_;

diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -56,16 +56,17 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/)
   loop_rate_ = get_parameter("loop_rate").as_int();
   waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string();
 
-  std::vector<std::string> new_args = rclcpp::NodeOptions().arguments();
-  new_args.push_back("--ros-args");
-  new_args.push_back("-r");
-  new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node");
-  new_args.push_back("--");
-  client_node_ = std::make_shared<rclcpp::Node>(
-    "_", "", rclcpp::NodeOptions().arguments(new_args));
+  callback_group_ = create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, get_node_base_interface());
 
   nav_to_pose_client_ = rclcpp_action::create_client<ClientT>(
-    client_node_, "navigate_to_pose");
+    get_node_base_interface(),
+    get_node_graph_interface(),
+    get_node_logging_interface(),
+    get_node_waitables_interface(),
+    "navigate_to_pose", callback_group_);
 
   action_server_ = std::make_unique<ActionServer>(
     get_node_base_interface(),
@@ -167,9 +168,9 @@ WaypointFollower::followWaypoints()
     // Check if asked to stop processing action
     if (action_server_->is_cancel_requested()) {
       auto cancel_future = nav_to_pose_client_->async_cancel_all_goals();
-      rclcpp::spin_until_future_complete(client_node_, cancel_future);
+      callback_group_executor_.spin_until_future_complete(cancel_future);
       // for result callback processing
-      spin_some(client_node_);
+      callback_group_executor_.spin_some();
       action_server_->terminate_all();
       return;
     }
@@ -267,7 +268,7 @@ WaypointFollower::followWaypoints()
         "Processing waypoint %i...", goal_index);
     }
 
-    rclcpp::spin_some(client_node_);
+    callback_group_executor_.spin_some();
     r.sleep();
   }
 }