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 846f8d1385..8c9fcb54d2 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_; 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 d51c7d2839..74ef275ddd 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_;