From 48a31f9370051b64d9ef2f01cd42f3dafe9ba706 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 24 Jun 2024 21:22:52 +0200 Subject: [PATCH] bt_service_node and bt_action_node: Don't block BT loop (backport #4214) (#4408) (#4475) * bt_service_node and bt_action_node: Don't block BT loop (#4214) * Set smaller timeout for service node * Fix timeout calculation for service node * Add a feasible timeout also for action node --------- * Increasing test count from timeout handling changes (#4234) --------- Signed-off-by: Christoph Froehlich Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- .../include/nav2_behavior_tree/bt_action_node.hpp | 9 ++++++--- .../include/nav2_behavior_tree/bt_service_node.hpp | 9 ++++++--- .../test/plugins/action/test_bt_action_node.cpp | 8 +++++--- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index a0b8021fe0..00755de96c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -56,7 +56,7 @@ 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("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); @@ -64,6 +64,9 @@ class BtActionNode : public BT::ActionNodeBase wait_for_service_timeout_ = config().blackboard->template get("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(bt_loop_duration * 0.5); + // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); @@ -404,7 +407,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; @@ -460,7 +463,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_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 052cb8b60b..b33314fa47 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -57,7 +57,7 @@ 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("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); @@ -65,6 +65,9 @@ class BtServiceNode : public BT::ActionNodeBase wait_for_service_timeout_ = config().blackboard->template get("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(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( @@ -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_; diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index a283dee33e..86455a24a8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/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