Use incremental timeouts in spin_until_future_calls during BT execution to handle large server timeouts

.gitignore

@@ -46,3 +46,7 @@ __pycache__/
 *.py[cod]
 
 sphinx_doc/_build
+
+# CLion artifacts
+.idea
+cmake-build-debug/

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -49,6 +49,8 @@ class BtActionNode : public BT::ActionNodeBase
     node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
 
     // Get the required items from the blackboard
+    bt_loop_duration_ =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
     server_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
@@ -174,7 +176,26 @@ class BtActionNode : public BT::ActionNodeBase
       // user defined callback
       on_tick();
 
-      on_new_goal_received();
+      send_new_goal();
+    }
+
+    // if new goal was sent and action server has not yet responded
+    // check the future goal handle
+    if (future_goal_handle_) {
+      auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
+      if (!is_future_goal_handle_complete(elapsed)) {
+        // return RUNNING if there is still some time before timeout happens
+        if (elapsed < server_timeout_) {
+          return BT::NodeStatus::RUNNING;
+        }
+        // if server has taken more time to respond than the specified timeout value return FAILURE
+        RCLCPP_WARN(
+          node_->get_logger(),
+          "Timed out while waiting for action server to acknowledge goal request for %s",
+          action_name_.c_str());
+        future_goal_handle_.reset();
+        return BT::NodeStatus::FAILURE;
+      }
     }
 
     // The following code corresponds to the "RUNNING" loop
@@ -187,7 +208,19 @@ class BtActionNode : public BT::ActionNodeBase
         goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
       {
         goal_updated_ = false;
-        on_new_goal_received();
+        send_new_goal();
+        auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
+        if (!is_future_goal_handle_complete(elapsed)) {
+          if (elapsed < server_timeout_) {
+            return BT::NodeStatus::RUNNING;
+          }
+          RCLCPP_WARN(
+            node_->get_logger(),
+            "Timed out while waiting for action server to acknowledge goal request for %s",
+            action_name_.c_str());
+          future_goal_handle_.reset();
+          return BT::NodeStatus::FAILURE;
+        }
       }
 
       rclcpp::spin_some(node_);
@@ -199,19 +232,26 @@ class BtActionNode : public BT::ActionNodeBase
       }
     }
 
+    BT::NodeStatus status;
     switch (result_.code) {
       case rclcpp_action::ResultCode::SUCCEEDED:
-        return on_success();
+        status = on_success();
+        break;
 
       case rclcpp_action::ResultCode::ABORTED:
-        return on_aborted();
+        status = on_aborted();
+        break;
 
       case rclcpp_action::ResultCode::CANCELED:
-        return on_cancelled();
+        status = on_cancelled();
+        break;
 
       default:
         throw std::logic_error("BtActionNode::Tick: invalid status value");
     }
+
+    goal_handle_.reset();
+    return status;
   }
 
   /**
@@ -246,6 +286,11 @@ class BtActionNode : public BT::ActionNodeBase
       return false;
     }
 
+    // No need to cancel the goal if goal handle is invalid
+    if (!goal_handle_) {
+      return false;
+    }
+
     rclcpp::spin_some(node_);
     auto status = goal_handle_->get_status();
 
@@ -257,7 +302,7 @@ class BtActionNode : public BT::ActionNodeBase
   /**
    * @brief Function to send new goal to action server
    */
-  void on_new_goal_received()
+  void send_new_goal()
   {
     goal_result_available_ = false;
     auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
@@ -272,18 +317,47 @@ class BtActionNode : public BT::ActionNodeBase
         }
       };
 
-    auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
+    future_goal_handle_ = std::make_shared<
+      std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
+      action_client_->async_send_goal(goal_, send_goal_options));
+    time_goal_sent_ = node_->now();
+  }
 
-    if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) !=
-      rclcpp::FutureReturnCode::SUCCESS)
-    {
+  /**
+   * @brief Function to check if the action server acknowledged a new goal
+   * @param elapsed Duration since the last goal was sent and future goal handle has not completed.
+   * After waiting for the future to complete, this value is incremented with the timeout value.
+   * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise
+   */
+  bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed)
+  {
+    auto remaining = server_timeout_ - elapsed;
+
+    // server has already timed out, no need to sleep
+    if (remaining <= std::chrono::milliseconds(0)) {
+      future_goal_handle_.reset();
+      return false;
+    }
+
+    auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
+    auto result = rclcpp::spin_until_future_complete(node_, *future_goal_handle_, timeout);
+    elapsed += timeout;
+
+    if (result == rclcpp::FutureReturnCode::INTERRUPTED) {
+      future_goal_handle_.reset();
       throw std::runtime_error("send_goal failed");
     }
 
-    goal_handle_ = future_goal_handle.get();
-    if (!goal_handle_) {
-      throw std::runtime_error("Goal was rejected by the action server");
+    if (result == rclcpp::FutureReturnCode::SUCCESS) {
+      goal_handle_ = future_goal_handle_->get();
+      future_goal_handle_.reset();
+      if (!goal_handle_) {
+        throw std::runtime_error("Goal was rejected by the action server");
+      }
+      return true;
     }
+
+    return false;
   }
 
   /**
@@ -313,6 +387,14 @@ class BtActionNode : public BT::ActionNodeBase
   // The timeout value while waiting for response from a server when a
   // new action goal is sent or canceled
   std::chrono::milliseconds server_timeout_;
+
+  // The timeout value for BT loop execution
+  std::chrono::milliseconds bt_loop_duration_;
+
+  // To track the action server acknowledgement when a new goal is sent
+  std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
+  future_goal_handle_;
+  rclcpp::Time time_goal_sent_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -225,6 +225,12 @@ class BtActionServer
   // To publish BT logs
   std::unique_ptr<RosTopicLogger> topic_logger_;
 
+  // Duration for each iteration of BT execution
+  std::chrono::milliseconds bt_loop_duration_;
+
+  // Default timeout value while waiting for response from a server
+  std::chrono::milliseconds default_server_timeout_;
+
   // Parameters for Groot monitoring
   bool enable_groot_monitoring_;
   int groot_zmq_publisher_port_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -53,6 +53,12 @@ BtActionServer<ActionT>::BtActionServer(
   clock_ = node->get_clock();
 
   // Declare this node's parameters
+  if (!node->has_parameter("bt_loop_duration")) {
+    node->declare_parameter("bt_loop_duration", 10);
+  }
+  if (!node->has_parameter("default_server_timeout")) {
+    node->declare_parameter("default_server_timeout", 20);
+  }
   if (!node->has_parameter("enable_groot_monitoring")) {
     node->declare_parameter("enable_groot_monitoring", true);
   }
@@ -91,6 +97,18 @@ bool BtActionServer<ActionT>::on_configure()
     node->get_node_waitables_interface(),
     action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this));
 
+  // Get parameter for monitoring with Groot via ZMQ Publisher
+  node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_);
+  node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_);
+  node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_);
+
+  // Get parameters for BT timeouts
+  int timeout;
+  node->get_parameter("bt_loop_duration", timeout);
+  bt_loop_duration_ = std::chrono::milliseconds(timeout);
+  node->get_parameter("default_server_timeout", timeout);
+  default_server_timeout_ = std::chrono::milliseconds(timeout);
+
   // Create the class that registers our custom nodes and executes the BT
   bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
 
@@ -99,12 +117,8 @@ bool BtActionServer<ActionT>::on_configure()
 
   // Put items on the blackboard
   blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
-  blackboard_->set<std::chrono::milliseconds>("server_timeout", std::chrono::milliseconds(10));  // NOLINT
-
-  // Get parameter for monitoring with Groot via ZMQ Publisher
-  node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_);
-  node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_);
-  node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_);
+  blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  // NOLINT
+  blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);  // NOLINT
 
   return true;
 }
@@ -217,7 +231,7 @@ void BtActionServer<ActionT>::executeCallback()
     };
 
   // Execute the BT that was previously created in the configure step
-  nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling);
+  nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling, bt_loop_duration_);
 
   // Make sure that the Bt is not in a running state from a previous execution
   // note: if all the ControlNodes are implemented correctly, this is not needed.

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -31,7 +31,7 @@ namespace nav2_behavior_tree
  * @tparam ServiceT Type of service
  */
 template<class ServiceT>
-class BtServiceNode : public BT::SyncActionNode
+class BtServiceNode : public BT::ActionNodeBase
 {
 public:
   /**
@@ -42,11 +42,13 @@ class BtServiceNode : public BT::SyncActionNode
   BtServiceNode(
     const std::string & service_node_name,
     const BT::NodeConfiguration & conf)
-  : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name)
+  : BT::ActionNodeBase(service_node_name, conf), service_node_name_(service_node_name)
   {
     node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
 
     // Get the required items from the blackboard
+    bt_loop_duration_ =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
     server_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
@@ -107,9 +109,22 @@ class BtServiceNode : public BT::SyncActionNode
    */
   BT::NodeStatus tick() override
   {
-    on_tick();
-    auto future_result = service_client_->async_send_request(request_);
-    return check_future(future_result);
+    if (!request_sent_) {
+      on_tick();
+      future_result_ = service_client_->async_send_request(request_);
+      sent_time_ = node_->now();
+      request_sent_ = true;
+    }
+    return check_future();
+  }
+
+  /**
+   * @brief The other (optional) override required by a BT service.
+   */
+  void halt() override
+  {
+    request_sent_ = false;
+    setStatus(BT::NodeStatus::IDLE);
   }
 
   /**
@@ -122,24 +137,35 @@ class BtServiceNode : public BT::SyncActionNode
 
   /**
    * @brief Check the future and decide the status of BT
-   * @param future_result shared_future of service response
    * @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise
    */
-  virtual BT::NodeStatus check_future(
-    std::shared_future<typename ServiceT::Response::SharedPtr> future_result)
+  virtual BT::NodeStatus check_future()
   {
-    rclcpp::FutureReturnCode rc;
-    rc = rclcpp::spin_until_future_complete(
-      node_,
-      future_result, server_timeout_);
-    if (rc == rclcpp::FutureReturnCode::SUCCESS) {
-      return BT::NodeStatus::SUCCESS;
-    } else if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
-      RCLCPP_WARN(
-        node_->get_logger(),
-        "Node timed out while executing service call to %s.", service_name_.c_str());
-      on_wait_for_result();
+    auto elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
+    auto remaining = server_timeout_ - elapsed;
+
+    if (remaining > std::chrono::milliseconds(0)) {
+      auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
+
+      auto rc = rclcpp::spin_until_future_complete(node_, future_result_, timeout);
+      if (rc == rclcpp::FutureReturnCode::SUCCESS) {
+        request_sent_ = false;
+        return BT::NodeStatus::SUCCESS;
+      }
+
+      if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
+        on_wait_for_result();
+        elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
+        if (elapsed < server_timeout_) {
+          return BT::NodeStatus::RUNNING;
+        }
+      }
     }
+
+    RCLCPP_WARN(
+      node_->get_logger(),
+      "Node timed out while executing service call to %s.", service_name_.c_str());
+    request_sent_ = false;
     return BT::NodeStatus::FAILURE;
   }
 
@@ -173,6 +199,14 @@ class BtServiceNode : public BT::SyncActionNode
   // The timeout value while to use in the tick loop while waiting for
   // a result from the server
   std::chrono::milliseconds server_timeout_;
+
+  // The timeout value for BT loop execution
+  std::chrono::milliseconds bt_loop_duration_;
+
+  // To track the server response when a new request is sent
+  std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
+  bool request_sent_{false};
+  rclcpp::Time sent_time_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -1,3 +1,6 @@
+ament_add_gtest(test_bt_action_node test_bt_action_node.cpp)
+ament_target_dependencies(test_bt_action_node ${dependencies})
+
 ament_add_gtest(test_action_spin_action test_spin_action.cpp)
 target_link_libraries(test_action_spin_action nav2_spin_action_bt_node)
 ament_target_dependencies(test_action_spin_action ${dependencies})

nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp

@@ -64,6 +64,9 @@ class BackUpActionTestFixture : public ::testing::Test
       node_);
     config_->blackboard->set<std::chrono::milliseconds>(
       "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
       std::chrono::milliseconds(10));
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<int>("number_recoveries", 0);