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 0f58ffe998b..3279a0144c1 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 @@ -137,11 +137,11 @@ class BtActionNode : public BT::ActionNodeBase } // The following code corresponds to the "RUNNING" loop + auto goal_status = goal_handle_->get_status(); if (rclcpp::ok() && !goal_result_available_) { // user defined callback. May modify the value of "goal_updated_" on_wait_for_result(); - auto goal_status = goal_handle_->get_status(); if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { @@ -163,7 +163,12 @@ class BtActionNode : public BT::ActionNodeBase return on_success(); case rclcpp_action::ResultCode::ABORTED: - return on_aborted(); + // TODO #1652 use PREEMPTED once rcl_action is updated + if (goal_status == action_msgs::msg::GoalStatus::STATUS_ABORTED) { + return on_aborted(); + } + // guess it is PREEMPTED because goal_status was replaced with a new goal + return BT::NodeStatus::RUNNING; case rclcpp_action::ResultCode::CANCELED: return on_cancelled(); @@ -214,10 +219,8 @@ class BtActionNode : public BT::ActionNodeBase auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); send_goal_options.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { - if (result.code != rclcpp_action::ResultCode::ABORTED) { - goal_result_available_ = true; - result_ = result; - } + goal_result_available_ = true; + result_ = result; }; auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);