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(); 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();