Skip to content
Snippets Groups Projects
Unverified Commit e8e6d944 authored by Carlos A. Orduno's avatar Carlos A. Orduno Committed by GitHub
Browse files

Using action client to get result instead of goal handle (#1310)

parent 2e28e23a
No related branches found
No related tags found
No related merge requests found
......@@ -104,12 +104,8 @@ public:
{
on_tick();
// Enable result awareness by providing an empty lambda function
auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
send_goal_options.result_callback = [](auto) {};
new_goal_received:
auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
auto future_goal_handle = action_client_->async_send_goal(goal_);
if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
rclcpp::executor::FutureReturnCode::SUCCESS)
{
......@@ -121,7 +117,7 @@ new_goal_received:
throw std::runtime_error("Goal was rejected by the action server");
}
auto future_result = goal_handle_->async_result();
auto future_result = action_client_->async_get_result(goal_handle_);
rclcpp::executor::FutureReturnCode rc;
do {
rc = rclcpp::spin_until_future_complete(node_, future_result, server_timeout_);
......
......@@ -134,13 +134,8 @@ LifecycleManagerClient::navigate_to_pose(double x, double y, double theta)
auto goal = nav2_msgs::action::NavigateToPose::Goal();
goal.pose = target_pose;
// Enable result awareness by providing an empty lambda function
auto send_goal_options =
typename rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
send_goal_options.result_callback = [](auto) {};
// Send it
auto future_goal_handle = navigate_action_client_->async_send_goal(goal, send_goal_options);
auto future_goal_handle = navigate_action_client_->async_send_goal(goal);
if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
rclcpp::executor::FutureReturnCode::SUCCESS)
{
......@@ -156,7 +151,8 @@ LifecycleManagerClient::navigate_to_pose(double x, double y, double theta)
}
// Wait for the action to complete
auto future_result = goal_handle->async_result();
auto future_result = navigate_action_client_->async_get_result(goal_handle);
if (rclcpp::spin_until_future_complete(node_, future_result) !=
rclcpp::executor::FutureReturnCode::SUCCESS)
{
......
......@@ -162,11 +162,7 @@ protected:
{
auto goal = RecoveryAction::Goal();
goal.command.data = command;
auto goal_options = rclcpp_action::Client<RecoveryAction>::SendGoalOptions();
goal_options.result_callback = [](auto) {};
return client_->async_send_goal(goal, goal_options);
return client_->async_send_goal(goal);
}
Status getOutcome()
......@@ -180,7 +176,7 @@ protected:
ClientGoalHandle::WrappedResult getResult()
{
auto future_result = goal_handle_->async_result();
auto future_result = client_->async_get_result(goal_handle_);
rclcpp::executor::FutureReturnCode frc;
do {
......
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