bt_service_node and bt_action_node: Don't block BT loop (ros-navigation#4214)

* Set smaller timeout for service node

Signed-off-by: Christoph Froehlich <[email protected]>

* Fix timeout calculation for service node

Signed-off-by: Christoph Froehlich <[email protected]>

* Add a feasible timeout also for action node

Signed-off-by: Christoph Froehlich <[email protected]>

---------

Signed-off-by: Christoph Froehlich <[email protected]>

Author: christophfroehlich

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -56,14 +56,17 @@ class BtActionNode : public BT::ActionNodeBase
     callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
     // Get the required items from the blackboard
-    bt_loop_duration_ =
+    auto 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_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
+    // timeout should be less than bt_loop_duration to be able to finish the current tick
+    max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
+
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
     result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
@@ -411,7 +414,7 @@ class BtActionNode : public BT::ActionNodeBase
       return false;
     }
 
-    auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
+    auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
     auto result =
       callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
     elapsed += timeout;
@@ -467,7 +470,7 @@ class BtActionNode : public BT::ActionNodeBase
   std::chrono::milliseconds server_timeout_;
 
   // The timeout value for BT loop execution
-  std::chrono::milliseconds bt_loop_duration_;
+  std::chrono::milliseconds max_timeout_;
 
   // The timeout value for waiting for a service to response
   std::chrono::milliseconds wait_for_service_timeout_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -57,14 +57,17 @@ class BtServiceNode : public BT::ActionNodeBase
     callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
     // Get the required items from the blackboard
-    bt_loop_duration_ =
+    auto 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_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
+    // timeout should be less than bt_loop_duration to be able to finish the current tick
+    max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
+
     // Now that we have node_ to use, create the service client for this BT service
     getInput("service_name", service_name_);
     service_client_ = node_->create_client<ServiceT>(
@@ -189,7 +192,7 @@ class BtServiceNode : public BT::ActionNodeBase
     auto remaining = server_timeout_ - elapsed;
 
     if (remaining > std::chrono::milliseconds(0)) {
-      auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
+      auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
 
       rclcpp::FutureReturnCode rc;
       rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout);
@@ -249,7 +252,7 @@ class BtServiceNode : public BT::ActionNodeBase
   std::chrono::milliseconds server_timeout_;
 
   // The timeout value for BT loop execution
-  std::chrono::milliseconds bt_loop_duration_;
+  std::chrono::milliseconds max_timeout_;
 
   // The timeout value for waiting for a service to response
   std::chrono::milliseconds wait_for_service_timeout_;