Add rcl count clients, servicec & tests

rcl/include/rcl/graph.h

@@ -584,6 +584,98 @@ rcl_count_subscribers(
   const char * topic_name,
   size_t * count);
 
+/// Return the number of clients on a given service.
+/**
+ * The `node` parameter must point to a valid node.
+ *
+ * The `service_name` parameter must not be `NULL`, and must not be an empty string.
+ * It should also follow the service name rules.
+ *
+ * See: https://design.ros2.org/articles/topic_and_service_names.html
+ *
+ * The `count` parameter must point to a valid size_t.
+ * The `count` parameter is the output for this function and will be set.
+ *
+ * In the event that error handling needs to allocate memory, this function
+ * will try to use the node's allocator.
+ *
+ * The service name is not automatically remapped by this function.
+ * If there is a client created with service name `foo` and remap rule `foo:=bar` then calling
+ * this with `service_name` set to `bar` will return a count of 1, and with `service_name` set to `foo`
+ * will return a count of 0.
+ * /sa rcl_remap_service_name()
+ *
+ * <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] service_name the name of the service in question
+ * \param[out] count number of clients on the given service
+ * \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_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_count_clients(
+  const rcl_node_t * node,
+  const char * service_name,
+  size_t * count);
+
+/// Return the number of servers on a given service.
+/**
+ * The `node` parameter must point to a valid node.
+ *
+ * The `service_name` parameter must not be `NULL`, and must not be an empty string.
+ * It should also follow the service name rules.
+ *
+ * See: https://design.ros2.org/articles/topic_and_service_names.html
+ *
+ * The `count` parameter must point to a valid size_t.
+ * The `count` parameter is the output for this function and will be set.
+ *
+ * In the event that error handling needs to allocate memory, this function
+ * will try to use the node's allocator.
+ *
+ * The service name is not automatically remapped by this function.
+ * If there is a server created with service name `foo` and remap rule `foo:=bar` then calling
+ * this with `service_name` set to `bar` will return a count of 1, and with `service_name` set to `foo`
+ * will return a count of 0.
+ * /sa rcl_remap_service_name()
+ *
+ * <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] service_name the name of the service in question
+ * \param[out] count number of services on the given service
+ * \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_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_count_services(
+  const rcl_node_t * node,
+  const char * service_name,
+  size_t * count);
+
 /// Wait for there to be a specified number of publishers on a given topic.
 /**
  * The `node` parameter must point to a valid node.

rcl/src/rcl/graph.c

@@ -456,6 +456,44 @@ rcl_count_subscribers(
   return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
 }
 
+rcl_ret_t
+rcl_count_clients(
+  const rcl_node_t * node,
+  const char * service_name,
+  size_t * count)
+{
+  if (!rcl_node_is_valid(node)) {
+    return RCL_RET_NODE_INVALID;  // error already set
+  }
+  const rcl_node_options_t * node_options = rcl_node_get_options(node);
+  if (!node_options) {
+    return RCL_RET_NODE_INVALID;  // shouldn't happen, but error is already set if so
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT);
+  rmw_ret_t rmw_ret = rmw_count_clients(rcl_node_get_rmw_handle(node), service_name, count);
+  return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+}
+
+rcl_ret_t
+rcl_count_services(
+  const rcl_node_t * node,
+  const char * service_name,
+  size_t * count)
+{
+  if (!rcl_node_is_valid(node)) {
+    return RCL_RET_NODE_INVALID;  // error already set
+  }
+  const rcl_node_options_t * node_options = rcl_node_get_options(node);
+  if (!node_options) {
+    return RCL_RET_NODE_INVALID;  // shouldn't happen, but error is already set if so
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT);
+  rmw_ret_t rmw_ret = rmw_count_services(rcl_node_get_rmw_handle(node), service_name, count);
+  return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+}
+
 typedef rcl_ret_t (* count_entities_func_t)(
   const rcl_node_t * node,
   const char * topic_name,

rcl/test/rcl/test_graph.cpp

@@ -716,6 +716,80 @@ TEST_F(
   rcl_reset_error();
 }
 
+/* Test the rcl_count_clients function.
+ *
+ * This does not test content of the response.
+ */
+TEST_F(
+  CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION),
+  test_rcl_count_clients
+) {
+  rcl_ret_t ret;
+  rcl_node_t zero_node = rcl_get_zero_initialized_node();
+  const char * service_name = "/topic_test_rcl_count_clients";
+  size_t count;
+  // invalid node
+  ret = rcl_count_clients(nullptr, service_name, &count);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  ret = rcl_count_clients(&zero_node, service_name, &count);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  ret = rcl_count_clients(this->old_node_ptr, service_name, &count);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // invalid topic name
+  ret = rcl_count_clients(this->node_ptr, nullptr, &count);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // TODO(wjwwood): test valid strings with invalid topic names in them
+  // invalid count
+  ret = rcl_count_clients(this->node_ptr, service_name, nullptr);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // valid call
+  ret = rcl_count_clients(this->node_ptr, service_name, &count);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+}
+
+/* Test the rcl_count_services function.
+ *
+ * This does not test content of the response.
+ */
+TEST_F(
+  CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION),
+  test_rcl_count_services
+) {
+  rcl_ret_t ret;
+  rcl_node_t zero_node = rcl_get_zero_initialized_node();
+  const char * service_name = "/topic_test_rcl_count_services";
+  size_t count;
+  // invalid node
+  ret = rcl_count_services(nullptr, service_name, &count);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  ret = rcl_count_services(&zero_node, service_name, &count);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  ret = rcl_count_services(this->old_node_ptr, service_name, &count);
+  EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // invalid topic name
+  ret = rcl_count_services(this->node_ptr, nullptr, &count);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // TODO(wjwwood): test valid strings with invalid topic names in them
+  // invalid count
+  ret = rcl_count_services(this->node_ptr, service_name, nullptr);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+  // valid call
+  ret = rcl_count_services(this->node_ptr, service_name, &count);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+}
+
 /* Test the rcl_wait_for_publishers function.
  */
 TEST_F(