add: wait for clients, servers

Signed-off-by: Minju, Lee <[email protected]>

Author: leeminju531

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.

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,

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,