From 8a533a5a304143a415da6d9bdecf5d2ffcd5ee82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 7 Jun 2024 19:52:45 +0000 Subject: [PATCH] bt_service_node and bt_action_node: Don't block BT loop (#4214) * Set smaller timeout for service node Signed-off-by: Christoph Froehlich * Fix timeout calculation for service node Signed-off-by: Christoph Froehlich * Add a feasible timeout also for action node Signed-off-by: Christoph Froehlich --------- Signed-off-by: Christoph Froehlich --- .../include/nav2_behavior_tree/bt_action_node.hpp | 9 ++++++--- .../include/nav2_behavior_tree/bt_service_node.hpp | 9 ++++++--- 2 files changed, 12 insertions(+), 6 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 5de3bff3bc..efa08aac16 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(); @@ -403,7 +406,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; @@ -459,7 +462,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 965b0943ec..57e69174cf 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_;