From 27db6f00f5f3e6f9218bc83c470838b9e52ab7d5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 26 Mar 2024 08:32:13 +0000 Subject: [PATCH 1/3] Set smaller timeout for service node Signed-off-by: Christoph Froehlich --- .../include/nav2_behavior_tree/bt_service_node.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) 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 d51c7d28398..1dd0c8703e6 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_ = + max_timeout_ = 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_ *= 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_; From fc330f8854d952c53ef8ee1cb6490f49a58d06ba Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 27 Mar 2024 13:18:42 +0000 Subject: [PATCH 2/3] Fix timeout calculation for service node Signed-off-by: Christoph Froehlich --- .../include/nav2_behavior_tree/bt_service_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 1dd0c8703e6..74ef275ddd5 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 - max_timeout_ = + auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); @@ -66,7 +66,7 @@ class BtServiceNode : public BT::ActionNodeBase 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_ *= 0.5; + 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_); From 44a1444bbc8861f135340cf8a0f1d9ecb09cc629 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 27 Mar 2024 13:17:29 +0000 Subject: [PATCH 3/3] Add a feasible timeout also for action node Signed-off-by: Christoph Froehlich --- .../include/nav2_behavior_tree/bt_action_node.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 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 846f8d13853..8c9fcb54d25 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(); @@ -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_;