Add clients servers info

rcl/include/rcl/graph.h

@@ -775,6 +775,80 @@ rcl_wait_for_subscribers(
   rcutils_duration_value_t timeout,
   bool * success);
 
+/// Wait for there to be a specified number of clients on a given service.
+/**
+ * \see rcl_wait_for_publishers
+ *
+ * <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] allocator to allocate space for the rcl_wait_set_t used to wait for graph events
+ * \param[in] service_name the name of the topic in question
+ * \param[in] count number of clients to wait for
+ * \param[in] timeout maximum duration to wait for clients
+ * \param[out] success `true` if the number of clients is equal to or greater than count, or
+ *   `false` if a timeout occurred waiting for clients.
+ * \return #RCL_RET_OK if there was no errors, or
+ * \return #RCL_RET_NODE_INVALID if the node is invalid, or
+ * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCL_RET_TIMEOUT if a timeout occurs before the number of clients is detected, or
+ * \return #RCL_RET_ERROR if an unspecified error occurred.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_wait_for_clients(
+  const rcl_node_t * node,
+  rcl_allocator_t * allocator,
+  const char * service_name,
+  const size_t count,
+  rcutils_duration_value_t timeout,
+  bool * success);
+
+/// Wait for there to be a specified number of servers on a given service.
+/**
+ * \see rcl_wait_for_publishers
+ *
+ * <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] allocator to allocate space for the rcl_wait_set_t used to wait for graph events
+ * \param[in] service_name the name of the topic in question
+ * \param[in] count number of servers to wait for
+ * \param[in] timeout maximum duration to wait for servers
+ * \param[out] success `true` if the number of servers is equal to or greater than count, or
+ *   `false` if a timeout occurred waiting for servers.
+ * \return #RCL_RET_OK if there was no errors, or
+ * \return #RCL_RET_NODE_INVALID if the node is invalid, or
+ * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCL_RET_TIMEOUT if a timeout occurs before the number of servers is detected, or
+ * \return #RCL_RET_ERROR if an unspecified error occurred.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_wait_for_services(
+  const rcl_node_t * node,
+  rcl_allocator_t * allocator,
+  const char * service_name,
+  const size_t count,
+  rcutils_duration_value_t timeout,
+  bool * success);
+
 /// Return a list of all publishers to a topic.
 /**
  * The `node` parameter must point to a valid node.
@@ -897,6 +971,128 @@ rcl_get_subscriptions_info_by_topic(
   bool no_mangle,
   rcl_topic_endpoint_info_array_t * subscriptions_info);
 
+/// Return a list of all clients to a service.
+/**
+ * The `node` parameter must point to a valid node.
+ *
+ * The `service_name` parameter must not be `NULL`.
+ *
+ * When the `no_mangle` parameter is `true`, the provided `service_name` should be a valid name
+ * for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps).
+ * When the `no_mangle` parameter is `false`, the provided `service_name` should follow
+ * ROS topic name conventions.
+ * In either case, the service name should always be fully qualified.
+ *
+ * Each element in the `clients_info` array will contain the node name, node namespace,
+ * service type, gid and the qos profile of the client.
+ * It is the responsibility of the caller to ensure that `clients_info` parameter points
+ * to a valid struct of type rcl_topic_endpoint_info_array_t.
+ * The `count` field inside the struct must be set to 0 and the `info_array` field inside
+ * the struct must be set to null.
+ * \see rmw_get_zero_initialized_topic_endpoint_info_array
+ *
+ * The `allocator` will be used to allocate memory to the `info_array` member
+ * inside of `clients_info`.
+ * Moreover, every const char * member inside of
+ * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory.
+ * \see rmw_topic_endpoint_info_set_node_name and the likes.
+ * However, it is the responsibility of the caller to
+ * reclaim any allocated resources to `clients_info` to avoid leaking memory.
+ * \see rmw_topic_endpoint_info_array_fini
+ *
+ * <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] allocator allocator to be used when allocating space for
+ *            the array inside clients_info
+ * \param[in] service_name the name of the service in question
+ * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware topic name,
+ *            otherwise it should be a valid ROS topic name
+ * \param[out] clients_info a struct representing a list of client information
+ * \return #RCL_RET_OK if the query was successful, or
+ * \return #RCL_RET_NODE_INVALID if the node is invalid, or
+ * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCL_RET_BAD_ALLOC if memory allocation fails, or
+ * \return #RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_get_clients_info_by_service(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * service_name,
+  bool no_mangle,
+  rcl_topic_endpoint_info_array_t * clients_info);
+
+/// Return a list of all servers to a service.
+/**
+ * The `node` parameter must point to a valid node.
+ *
+ * The `service_name` parameter must not be `NULL`.
+ *
+ * When the `no_mangle` parameter is `true`, the provided `service_name` should be a valid name
+ * for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps).
+ * When the `no_mangle` parameter is `false`, the provided `service_name` should follow
+ * ROS topic name conventions.
+ * In either case, the service name should always be fully qualified.
+ *
+ * Each element in the `servers_info` array will contain the node name, node namespace,
+ * service type, gid and the qos profile of the server.
+ * It is the responsibility of the caller to ensure that `servers_info` parameter points
+ * to a valid struct of type rcl_topic_endpoint_info_array_t.
+ * The `count` field inside the struct must be set to 0 and the `info_array` field inside
+ * the struct must be set to null.
+ * \see rmw_get_zero_initialized_topic_endpoint_info_array
+ *
+ * The `allocator` will be used to allocate memory to the `info_array` member
+ * inside of `servers_info`.
+ * Moreover, every const char * member inside of
+ * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory.
+ * \see rmw_topic_endpoint_info_set_node_name and the likes.
+ * However, it is the responsibility of the caller to
+ * reclaim any allocated resources to `servers_info` to avoid leaking memory.
+ * \see rmw_topic_endpoint_info_array_fini
+ *
+ * <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] allocator allocator to be used when allocating space for
+ *            the array inside clients_info
+ * \param[in] service_name the name of the service in question
+ * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware topic name,
+ *            otherwise it should be a valid ROS topic name
+ * \param[out] servers_info a struct representing a list of server information
+ * \return #RCL_RET_OK if the query was successful, or
+ * \return #RCL_RET_NODE_INVALID if the node is invalid, or
+ * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCL_RET_BAD_ALLOC if memory allocation fails, or
+ * \return #RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_get_servers_info_by_service(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * service_name,
+  bool no_mangle,
+  rcl_topic_endpoint_info_array_t * servers_info);
+
 /// Check if a service server is available for the given service client.
 /**
  * This function will return true for `is_available` if there is a service server

rcl/include/rcl/init.h

@@ -50,7 +50,7 @@ extern "C"
  *
  * The `options` argument must be non-`NULL` and must have been initialized
  * with rcl_init_options_init().
- * It is unmodified by this function, and the ownership is not transfered to
+ * It is unmodified by this function, and the ownership is not transferred to
  * the context, but instead a copy is made into the context for later reference.
  * Therefore, the given options need to be cleaned up with
  * rcl_init_options_fini() after this function returns.

rcl/src/rcl/graph.c

@@ -667,6 +667,44 @@ rcl_wait_for_subscribers(
     rcl_count_subscribers);
 }
 
+rcl_ret_t
+rcl_wait_for_clients(
+  const rcl_node_t * node,
+  rcl_allocator_t * allocator,
+  const char * service_name,
+  const size_t expected_count,
+  rcutils_duration_value_t timeout,
+  bool * success)
+{
+  return _rcl_wait_for_entities(
+    node,
+    allocator,
+    service_name,
+    expected_count,
+    timeout,
+    success,
+    rcl_count_clients);
+}
+
+rcl_ret_t
+rcl_wait_for_services(
+  const rcl_node_t * node,
+  rcl_allocator_t * allocator,
+  const char * service_name,
+  const size_t expected_count,
+  rcutils_duration_value_t timeout,
+  bool * success)
+{
+  return _rcl_wait_for_entities(
+    node,
+    allocator,
+    service_name,
+    expected_count,
+    timeout,
+    success,
+    rcl_count_services);
+}
+
 typedef rmw_ret_t (* get_topic_endpoint_info_func_t)(
   const rmw_node_t * node,
   rcutils_allocator_t * allocator,
@@ -751,6 +789,40 @@ rcl_get_subscriptions_info_by_topic(
     rmw_get_subscriptions_info_by_topic);
 }
 
+rcl_ret_t
+rcl_get_clients_info_by_service(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * service_name,
+  bool no_mangle,
+  rmw_topic_endpoint_info_array_t * clients_info)
+{
+  return __rcl_get_info_by_topic(
+    node,
+    allocator,
+    service_name,
+    no_mangle,
+    clients_info,
+    rmw_get_clients_info_by_service);
+}
+
+rcl_ret_t
+rcl_get_servers_info_by_service(
+  const rcl_node_t * node,
+  rcutils_allocator_t * allocator,
+  const char * service_name,
+  bool no_mangle,
+  rmw_topic_endpoint_info_array_t * servers_info)
+{
+  return __rcl_get_info_by_topic(
+    node,
+    allocator,
+    service_name,
+    no_mangle,
+    servers_info,
+    rmw_get_servers_info_by_service);
+}
+
 rcl_ret_t
 rcl_service_server_is_available(
   const rcl_node_t * node,

rcl/test/rcl/test_graph.cpp

@@ -834,6 +834,90 @@ TEST_F(TestGraphFixture, test_rcl_wait_for_subscribers) {
   rcl_reset_error();
 }
 
+/* Test the rcl_wait_for_clients function.
+ */
+TEST_F(TestGraphFixture, test_rcl_wait_for_clients) {
+  rcl_ret_t ret;
+  rcl_node_t zero_node = rcl_get_zero_initialized_node();
+  rcl_allocator_t zero_allocator = static_cast<rcl_allocator_t>(
+    rcutils_get_zero_initialized_allocator());
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  const char * service_name = "/topic_test_rcl_wait_for_clients";
+  bool success = false;
+
+  // Invalid node
+  ret = rcl_wait_for_clients(nullptr, &allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret);
+  rcl_reset_error();
+  ret = rcl_wait_for_clients(&zero_node, &allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret);
+  rcl_reset_error();
+  ret = rcl_wait_for_clients(this->old_node_ptr, &allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // Invalid allocator
+  ret = rcl_wait_for_clients(this->node_ptr, nullptr, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  ret = rcl_wait_for_clients(this->node_ptr, &zero_allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // Invalid topic name
+  ret = rcl_wait_for_clients(this->node_ptr, &allocator, nullptr, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // Invalid output arg
+  ret = rcl_wait_for_clients(this->node_ptr, &allocator, service_name, 1u, 100, nullptr);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // Valid call (expect timeout since there are no clients)
+  ret = rcl_wait_for_clients(this->node_ptr, &allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+}
+
+/* Test the rcl_wait_for_clients function.
+ */
+TEST_F(TestGraphFixture, test_rcl_wait_for_services) {
+  rcl_ret_t ret;
+  rcl_node_t zero_node = rcl_get_zero_initialized_node();
+  rcl_allocator_t zero_allocator = static_cast<rcl_allocator_t>(
+    rcutils_get_zero_initialized_allocator());
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  const char * service_name = "/topic_test_rcl_wait_for_services";
+  bool success = false;
+
+  // Invalid node
+  ret = rcl_wait_for_services(nullptr, &allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret);
+  rcl_reset_error();
+  ret = rcl_wait_for_services(&zero_node, &allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret);
+  rcl_reset_error();
+  ret = rcl_wait_for_services(this->old_node_ptr, &allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // Invalid allocator
+  ret = rcl_wait_for_services(this->node_ptr, nullptr, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  ret = rcl_wait_for_services(this->node_ptr, &zero_allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // Invalid topic name
+  ret = rcl_wait_for_services(this->node_ptr, &allocator, nullptr, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // Invalid output arg
+  ret = rcl_wait_for_services(this->node_ptr, &allocator, service_name, 1u, 100, nullptr);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // Valid call (expect timeout since there are no servers)
+  ret = rcl_wait_for_services(this->node_ptr, &allocator, service_name, 1u, 100, &success);
+  EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+}
+
 void
 check_entity_count(
   const rcl_node_t * node_ptr,