diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 172f0a7f4aaaea951a50b500e6f778281702384d..52cb37b5cb23abac5eb33cec23c4a4e8beeebf25 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -41,11 +41,6 @@ public: goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult(); - // Get the required items from the blackboard - server_timeout_ = - config().blackboard->get<std::chrono::milliseconds>("server_timeout"); - getInput<std::chrono::milliseconds>("server_timeout", server_timeout_); - createActionClient(action_name_); // Give the derive class a chance to do any initialization @@ -87,7 +82,7 @@ public: } // Derived classes can override any of the following methods to hook into the - // processing for the action: on_tick, on_server_timeout, and on_success + // processing for the action: on_tick, on_wait_for_result, and on_success // Could do dynamic checks, such as getting updates to values on the blackboard virtual void on_tick() @@ -96,7 +91,7 @@ public: // There can be many loop iterations per tick. Any opportunity to do something after // a timeout waiting for a result that hasn't been received yet - virtual void on_server_timeout() + virtual void on_wait_for_result() { } @@ -112,7 +107,18 @@ public: on_tick(); new_goal_received: - auto future_goal_handle = action_client_->async_send_goal(goal_); + goal_result_available_ = false; + auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions(); + send_goal_options.result_callback = + [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) { + if (result.code != rclcpp_action::ResultCode::ABORTED) { + goal_result_available_ = true; + result_ = result; + } + }; + + auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + if (rclcpp::spin_until_future_complete(node_, future_goal_handle) != rclcpp::executor::FutureReturnCode::SUCCESS) { @@ -124,28 +130,22 @@ new_goal_received: throw std::runtime_error("Goal was rejected by the action server"); } - 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_); - if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) { - on_server_timeout(); - - // We can handle a new goal if we're still executing - auto status = goal_handle_->get_status(); - if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || - status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED )) - { - goal_updated_ = false; - goto new_goal_received; - } + while (rclcpp::ok() && !goal_result_available_) { + on_wait_for_result(); - // Yield to any other CoroActionNodes (coroutines) - setStatusRunningAndYield(); + auto status = goal_handle_->get_status(); + if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + { + goal_updated_ = false; + goto new_goal_received; } - } while (rc != rclcpp::executor::FutureReturnCode::SUCCESS); - result_ = future_result.get(); + // Yield to any other nodes + setStatusRunningAndYield(); + rclcpp::spin_some(node_); + } + switch (result_.code) { case rclcpp_action::ResultCode::SUCCEEDED: on_success(); @@ -211,15 +211,12 @@ protected: // All ROS2 actions have a goal and a result typename ActionT::Goal goal_; bool goal_updated_{false}; + bool goal_result_available_{false}; typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_; typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_; // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; - - // The timeout value while to use in the tick loop while waiting for - // a result from the server - std::chrono::milliseconds server_timeout_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 93c40ff47efc2e095225640b0c8a55201a2f3f05..39ef9c191baa7139728a273acc9dd34fe5b48593 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -96,7 +96,7 @@ public: RCLCPP_WARN( node_->get_logger(), "Node timed out while executing service call to %s.", service_name_.c_str()); - on_server_timeout(); + on_wait_for_result(); } return BT::NodeStatus::FAILURE; } @@ -109,7 +109,7 @@ public: // An opportunity to do something after // a timeout waiting for a result that hasn't been received yet - virtual void on_server_timeout() + virtual void on_wait_for_result() { } diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 6f056b921faa111953bcca462f96b58c3a5f6f81..d3cbbdafe8fb86fed6d7a555d58c255d1697a3da 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -47,7 +47,7 @@ public: getInput("controller_id", goal_.controller_id); } - void on_server_timeout() override + void on_wait_for_result() override { // Check if the goal has been updated if (config().blackboard->get<bool>("path_updated")) {