From 21d36bfd1321b97750a82022f6a3bf38bb28ad54 Mon Sep 17 00:00:00 2001 From: Vinny Ruia Date: Wed, 22 Jun 2022 13:31:03 -0400 Subject: [PATCH] adding timeout for action client initialization (#3029) * adding timeout for action client initialization Signed-off-by: Vinny Ruia * adding constant 1s timeout, catching exception Signed-off-by: Vinny Ruia --- .../include/nav2_behavior_tree/bt_action_node.hpp | 9 ++++++++- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 +++++++- .../src/navigators/navigate_through_poses.cpp | 2 +- 3 files changed, 16 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 5b34469e7ef..8de0b27977b 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 @@ -90,7 +90,14 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - action_client_->wait_for_action_server(); + static constexpr std::chrono::milliseconds wait_for_server_timeout = + std::chrono::milliseconds(1000); + if (!action_client_->wait_for_action_server(wait_for_server_timeout)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" action server not available after waiting for %li ms", + action_name.c_str(), wait_for_server_timeout.count()); + throw std::runtime_error("Action server not available"); + } } /** diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 37b2b809ddd..592d1257a27 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -172,7 +172,13 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena std::istreambuf_iterator()); // Create the Behavior Tree from the XML input - tree_ = bt_->createTreeFromText(xml_string, blackboard_); + try { + tree_ = bt_->createTreeFromText(xml_string, blackboard_); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); + return false; + } + topic_logger_ = std::make_unique(client_node_, tree_); current_bt_xml_filename_ = filename; diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 237f6ba8ba7..34a3f429a49 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -76,7 +76,7 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { RCLCPP_ERROR( - logger_, "BT file not found: %s. Navigation canceled.", + logger_, "Error loading XML file: %s. Navigation canceled.", bt_xml_filename.c_str()); return false; }