diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h index 0837583..2a6b8fa 100644 --- a/rcl_action/include/rcl_action/action_client.h +++ b/rcl_action/include/rcl_action/action_client.h @@ -229,6 +229,50 @@ RCL_WARN_UNUSED rcl_action_client_options_t rcl_action_client_get_default_options(void); +/// Check if an action server is available for the given action client. +/** + * This function will return true for is_available if there is an action server + * available for the given action client. + * + * The node parameter must not be `NULL`, and must point to a valid node. + * + * The client parameter must not be `NULL`, and must point to a valid client. + * + * The given client and node must match, i.e. the client must have been created + * using the given node. + * + * The is_available parameter must not be `NULL`, and must point a bool variable. + * The result of the check will be stored in the is_available parameter. + * + * In the event that error handling needs to allocate memory, this function + * will try to use the node's allocator. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] client the handle to the action client being queried + * \param[out] is_available set to true if there is an action server available, else false + * \return `RCL_RET_OK` if successful (regardless of the action server availability), or + * \return `RCL_RET_NODE_INVALID` if the node is invalid, or + * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action client is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_server_is_available( + const rcl_node_t * node, + const rcl_action_client_t * client, + bool * is_available); + /// Send a ROS goal using a rcl_action_client_t. /** * This is a non-blocking call. diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 500587a..ae97bed 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -26,6 +26,7 @@ extern "C" #include "rcl/client.h" #include "rcl/error_handling.h" +#include "rcl/graph.h" #include "rcl/subscription.h" #include "rcl/types.h" #include "rcl/wait.h" @@ -239,6 +240,54 @@ rcl_action_client_get_default_options(void) return default_options; } +rcl_ret_t +rcl_action_server_is_available( + const rcl_node_t * node, + const rcl_action_client_t * client, + bool * is_available) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error is already set + } + if (!rcl_action_client_is_valid(client)) { + return RCL_RET_ACTION_CLIENT_INVALID; // error is already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(is_available, RCL_RET_INVALID_ARGUMENT); + + bool temp; + rcl_ret_t ret; + *is_available = true; + + ret = rcl_service_server_is_available(node, &(client->impl->goal_client), &temp); + if (RCL_RET_OK != ret) { + return ret; // error is already set + } + *is_available = (*is_available && temp); + + ret = rcl_service_server_is_available(node, &(client->impl->cancel_client), &temp); + if (RCL_RET_OK != ret) { + return ret; // error is already set + } + *is_available = (*is_available && temp); + + ret = rcl_service_server_is_available(node, &(client->impl->result_client), &temp); + if (RCL_RET_OK != ret) { + return ret; // error is already set + } + *is_available = (*is_available && temp); + + size_t number_of_publishers; + + ret = rcl_subscription_get_publisher_count( + &(client->impl->feedback_subscription), &number_of_publishers); + if (RCL_RET_OK != ret) { + return ret; // error is already set + } + *is_available = *is_available && (number_of_publishers != 0); + + return RCL_RET_OK; +} + // \internal Sends an action client specific service request. #define SEND_SERVICE_REQUEST(Type, request, sequence_number) \ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action " #Type " request"); \