bt_service_node and bt_action_node: Don't block BT loop (backport #4214…

…) (#4408)

* bt_service_node and bt_action_node: Don't block BT loop (#4214)

* Set smaller timeout for service node

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* Fix timeout calculation for service node

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* Add a feasible timeout also for action node

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

---------

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* Increasing test count from timeout handling changes (#4234)

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

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();
@@ -405,7 +408,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;
@@ -461,7 +464,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_;

nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp

@@ -316,8 +316,10 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success)
   // the BT should have failed
   EXPECT_EQ(result, BT::NodeStatus::FAILURE);
 
-  // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2
-  EXPECT_EQ(ticks, 2);
+  // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should
+  // be at most 2, but it can be 1 too, because the tickOnce may execute two ticks.
+  EXPECT_LE(ticks, 3);
+  EXPECT_GE(ticks, 1);
 }
 
 TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
@@ -362,7 +364,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
   EXPECT_EQ(result, BT::NodeStatus::FAILURE);
 
   // since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9
-  EXPECT_EQ(ticks, 9);
+  EXPECT_EQ(ticks, 10);
 
   // start a new execution cycle with the previous BT to ensure previous state doesn't leak into
   // the new cycle