From fa88917c71149b69473aec244a8999d490685d38 Mon Sep 17 00:00:00 2001 From: Vinny Ruia Date: Mon, 20 Jun 2022 21:09:24 -0400 Subject: [PATCH 1/2] adding timeout for action client initialization Signed-off-by: Vinny Ruia --- .../include/nav2_behavior_tree/bt_action_node.hpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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..eaea9244b48 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,12 @@ 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(); + if (!action_client_->wait_for_action_server(server_timeout_)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" action server not available after waiting for %li ms", + action_name.c_str(), server_timeout_.count()); + throw std::runtime_error("Action server not available"); + } } /** From 56e276f2730da57eed989e51e6ddc3997bdb0e74 Mon Sep 17 00:00:00 2001 From: Vinny Ruia Date: Mon, 20 Jun 2022 22:17:25 -0400 Subject: [PATCH 2/2] adding constant 1s timeout, catching exception Signed-off-by: Vinny Ruia --- .../include/nav2_behavior_tree/bt_action_node.hpp | 6 ++++-- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 +++++++- .../src/navigators/navigate_through_poses.cpp | 2 +- 3 files changed, 12 insertions(+), 4 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 eaea9244b48..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,10 +90,12 @@ 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()); - if (!action_client_->wait_for_action_server(server_timeout_)) { + 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(), server_timeout_.count()); + 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; }