From c0d2787e712497b957d7dad5644245184c7d33ad Mon Sep 17 00:00:00 2001 From: bi0ha2ard Date: Wed, 13 Mar 2024 19:38:04 +0100 Subject: [PATCH] chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178) Signed-off-by: Felix Co-authored-by: Felix Signed-off-by: Kemal Bektas --- .../include/nav2_behavior_tree/bt_action_node.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 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 a7a04a3055..d34f609724 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 @@ -97,8 +97,9 @@ class BtActionNode : public BT::ActionNodeBase RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for 1s", - action_name.c_str()); + node_->get_logger(), "\"%s\" action server not available after waiting for %f s", + action_name.c_str(), + wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string("Action server ") + action_name + std::string(" not available"));