From 8f7f4f0804a34ee9d9ecd2d7e75a57ce2b7ced5d Mon Sep 17 00:00:00 2001 From: "Minju, Lee" <70446214+leeminju531@users.noreply.github.com> Date: Thu, 5 Oct 2023 11:29:13 +0900 Subject: [PATCH] Add rcl count clients, servicec & tests (#1011) * Add rcl count clients, services and test Signed-off-by: leeminju531 --- rcl/include/rcl/graph.h | 92 +++++++++++++++++++++++++++++++++++++ rcl/src/rcl/graph.c | 38 +++++++++++++++ rcl/test/rcl/test_graph.cpp | 74 +++++++++++++++++++++++++++++ 3 files changed, 204 insertions(+) diff --git a/rcl/include/rcl/graph.h b/rcl/include/rcl/graph.h index 13133b6ba..2049359c8 100644 --- a/rcl/include/rcl/graph.h +++ b/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() + * + *
+ * 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] 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() + * + *
+ * 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] 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. diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c index 5ba0a0a67..323f0199b 100644 --- a/rcl/src/rcl/graph.c +++ b/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, diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 742dba992..1d42c7acc 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/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(