add wait_for_action_server() for action clients (ros2#349)

Signed-off-by: William Woodall <william@osrfoundation.org>

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.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Maybe [1]
+ * <i>[1] implementation may need to protect the data structure with a lock</i>
+ *
+ * \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.

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"); \