a workaround for ros-navigation#1652 issue

Signed-off-by: Daisuke Sato <[email protected]>

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<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;
-        }
+        goal_result_available_ = true;
+        result_ = result;
       };
 
     auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);