BT Service Node to throw if service was not available in time (ros-na…

…vigation#3256) * throw if service server wasn't available in time mimic the behavior of the bt action node constructor * throw if action unavailable in bt cancel action * use chrono literals namespace * fix linting errors * fix code style divergence
jwallace42 · Dec 14, 2022 · 61a967c · 61a967c
1 parent ae268d8
commit 61a967c
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 2 deletions.
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
@@ -27,6 +27,8 @@
 namespace nav2_behavior_tree
 {
 
+using namespace std::chrono_literals;  // NOLINT
+
 /**
  * @brief Abstract class representing an action for cancelling BT node
  * @tparam ActionT Type of action
@@ -87,7 +89,12 @@ class BtCancelActionNode : 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(1s)) {
+      RCLCPP_ERROR(
+        node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
+        action_name.c_str());
+      throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
+    }
   }
 
   /**

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
@@ -17,6 +17,7 @@
 
 #include <string>
 #include <memory>
+#include <chrono>
 
 #include "behaviortree_cpp_v3/action_node.h"
 #include "nav2_util/node_utils.hpp"
@@ -26,6 +27,8 @@
 namespace nav2_behavior_tree
 {
 
+using namespace std::chrono_literals;  // NOLINT
+
 /**
  * @brief Abstract class representing a service based BT node
  * @tparam ServiceT Type of service
@@ -71,7 +74,15 @@ class BtServiceNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(
       node_->get_logger(), "Waiting for \"%s\" service",
       service_name_.c_str());
-    service_client_->wait_for_service();
+    if (!service_client_->wait_for_service(1s)) {
+      RCLCPP_ERROR(
+        node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
+        service_node_name.c_str());
+      throw std::runtime_error(
+              std::string(
+                "Service server %s not available",
+                service_node_name.c_str()));
+    }
 
     RCLCPP_DEBUG(
       node_->get_logger(), "\"%s\" BtServiceNode initialized",