Revert "Fix BT action client implementation (#1384)" (#1406)

This reverts commit 876b833e.

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -81,7 +81,7 @@ public:
   }
 
   // Derived classes can override any of the following methods to hook into the
-  // processing for the action: on_tick, on_wait_for_result, and on_success
+  // processing for the action: on_tick, on_server_timeout, and on_success
 
   // Could do dynamic checks, such as getting updates to values on the blackboard
   virtual void on_tick()
@@ -90,7 +90,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_wait_for_result()
+  virtual void on_server_timeout()
   {
   }
 
@@ -106,17 +106,7 @@ public:
     on_tick();
 
 new_goal_received:
-
-    bool goal_result_available = false;
-    auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
-    send_goal_options.result_callback =
-      [&](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
-        goal_result_available = true;
-        result_ = result;
-      };
-
-    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)
     {
@@ -128,18 +118,28 @@ new_goal_received:
       throw std::runtime_error("Goal was rejected by the action server");
     }
 
-    while (rclcpp::ok() && !goal_result_available) {
-      on_wait_for_result();
-      if (goal_updated_) {
-        goal_updated_ = false;
-        goto new_goal_received;
+    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;
+        }
+
+        // Yield to any other CoroActionNodes (coroutines)
+        setStatusRunningAndYield();
       }
+    } while (rc != rclcpp::executor::FutureReturnCode::SUCCESS);
 
-      // Yield to any other nodes
-      setStatusRunningAndYield();
-      rclcpp::spin_some(node_);
-    }
-
+    result_ = future_result.get();
     switch (result_.code) {
       case rclcpp_action::ResultCode::SUCCEEDED:
         on_success();

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -92,7 +92,7 @@ public:
     } else if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
       RCLCPP_WARN(node_->get_logger(),
         "Node timed out while executing service call to %s.", service_name_.c_str());
-      on_wait_for_result();
+      on_server_timeout();
     }
     return BT::NodeStatus::FAILURE;
   }
@@ -105,7 +105,7 @@ public:
 
   // An opportunity to do something after
   // a timeout waiting for a result that hasn't been received yet
-  virtual void on_wait_for_result()
+  virtual void on_server_timeout()
   {
   }
 

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -42,7 +42,7 @@ public:
     getInput("controller_id", goal_.controller_id);
   }
 
-  void on_wait_for_result() override
+  void on_server_timeout() override
   {
     // Check if the goal has been updated
     if (config().blackboard->get<bool>("path_updated")) {