From e52663aa85795b5d6c64ae1fe0cb1d534eb8c596 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 31 Jan 2024 16:38:28 -0600 Subject: [PATCH 1/2] Check service server before calling --- .../include/behaviortree_ros2/bt_service_node.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 66477e3..67a6750 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -278,6 +278,10 @@ template inline return CheckStatus( onFailure(INVALID_REQUEST) ); } + // Check if server is ready + if(!service_client_->service_is_ready()) + return onFailure(SERVICE_UNREACHABLE); + future_response_ = service_client_->async_send_request(request).share(); time_request_sent_ = node_->now(); From f5513d8a70fd0261b3f1540202334b0c18d7506c Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 31 Jan 2024 15:44:30 -0600 Subject: [PATCH 2/2] Check action before calling --- .../include/behaviortree_ros2/bt_action_node.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 8e4d49f..c6b0c97 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -352,6 +352,10 @@ template inline }; //-------------------- + // Check if server is ready + if(!action_client_->action_server_is_ready()) + return onFailure(SERVER_UNREACHABLE); + future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); time_goal_sent_ = node_->now();