Skip to content
Snippets Groups Projects
Unverified Commit 54911a70 authored by 戈振鹏's avatar 戈振鹏 Committed by GitHub
Browse files

Reduce nodes for nav2_waypoint_follower (#2441)


* Reduce node of waypoint_follower by using callback group and executor

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

* fix linting failures

Signed-off-by: default avatarzhenpeng ge <zhenpeng.ge@qq.com>
parent 80f4ef83
No related branches found
No related tags found
No related merge requests found
......@@ -118,7 +118,8 @@ protected:
// 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_;
......
......@@ -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();
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment