From 1fb1f9e53bec9b08452da4cd558b252dcd7c3153 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Fri, 11 Nov 2022 19:11:11 +0100 Subject: [PATCH] BT Service Node to throw if service was not available in time (#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 --- .../nav2_behavior_tree/bt_cancel_action_node.hpp | 9 ++++++++- .../include/nav2_behavior_tree/bt_service_node.hpp | 13 ++++++++++++- 2 files changed, 20 insertions(+), 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 index b48478ecc48..504df9157a1 100644 --- 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 index b99e5b74d41..91b0c0cd9d8 100644 --- 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 #include +#include #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",