diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 1abbdba75b..ab1f76baab 100644 --- 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 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::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 index 3dbb7d21f1..e95203d628 100644 --- 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 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::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( - 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( 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(); } }