adding timeout for action client initialization (ros-navigation#3029)

* adding timeout for action client initialization Signed-off-by: Vinny Ruia <vinny.ruia@fireflyautomatix.com> * adding constant 1s timeout, catching exception Signed-off-by: Vinny Ruia <vinny.ruia@fireflyautomatix.com>
jwallace42 · Dec 14, 2022 · 21d36bf · 21d36bf
1 parent ef0102b
commit 21d36bf
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 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
@@ -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
@@ -172,7 +172,13 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     std::istreambuf_iterator<char>());
 
   // 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<RosTopicLogger>(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
@@ -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;
   }