From ed141ef1a1dc70ee8f53b2cfca1a857220bf24fa Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 20 Jul 2026 08:02:14 +0900 Subject: [PATCH 1/4] introduce rcl_action_endpoint_info_array_t. Signed-off-by: Tomoya Fujita --- rcl_action/include/rcl_action/graph.h | 177 ++++++++++++ rcl_action/src/rcl_action/graph.c | 316 ++++++++++++++++++++++ rcl_action/test/rcl_action/test_graph.cpp | 268 ++++++++++++++++++ 3 files changed, 761 insertions(+) diff --git a/rcl_action/include/rcl_action/graph.h b/rcl_action/include/rcl_action/graph.h index f2a5c5cfc..d8bba4ffc 100644 --- a/rcl_action/include/rcl_action/graph.h +++ b/rcl_action/include/rcl_action/graph.h @@ -239,6 +239,183 @@ rcl_action_count_servers( const char * action_name, size_t * count); +/// Endpoint information of an action client or an action server. +/** + * An action is built on top of three services and two topics: + * + * - the goal service (`/_action/send_goal`) + * - the cancel service (`/_action/cancel_goal`) + * - the result service (`/_action/get_result`) + * - the feedback topic (`/_action/feedback`) + * - the status topic (`/_action/status`) + * + * This structure aggregates the endpoint information of all the underlying + * entities of one action client or one action server. + * The goal service endpoint is used as the canonical identity of an action + * client or an action server, so `goal_service_info` is always populated. + * The remaining endpoint information is correlated to the goal service + * endpoint by the node name and node namespace. + * If an underlying entity has not been discovered (yet), or if the + * correlation is not possible, the corresponding member is left zero + * initialized (i.e. its `node_name` is `NULL`). + * + * The `service_type` and `topic_type` fields hold the types of the + * underlying entities (e.g. `test_msgs/action/Fibonacci_SendGoal`), not the + * action type. + * The action type can be derived by trimming the `_SendGoal` suffix from the + * goal service type. + */ +typedef struct rcl_action_endpoint_info_s +{ + /// Endpoint information of the goal service of the action client or server. + rcl_service_endpoint_info_t goal_service_info; + /// Endpoint information of the cancel service of the action client or server. + rcl_service_endpoint_info_t cancel_service_info; + /// Endpoint information of the result service of the action client or server. + rcl_service_endpoint_info_t result_service_info; + /// Endpoint information of the feedback topic of the action client or server. + rcl_topic_endpoint_info_t feedback_topic_info; + /// Endpoint information of the status topic of the action client or server. + rcl_topic_endpoint_info_t status_topic_info; +} rcl_action_endpoint_info_t; + +/// Array of rcl_action_endpoint_info_t. +typedef struct rcl_action_endpoint_info_array_s +{ + /// Size of the array. + size_t size; + /// Contiguous storage of the array. + rcl_action_endpoint_info_t * info_array; +} rcl_action_endpoint_info_array_t; + +/// Return a rcl_action_endpoint_info_t with members set to `NULL` or zero. +RCL_ACTION_PUBLIC +rcl_action_endpoint_info_t +rcl_action_get_zero_initialized_endpoint_info(void); + +/// Return a rcl_action_endpoint_info_array_t with members set to `NULL` or zero. +RCL_ACTION_PUBLIC +rcl_action_endpoint_info_array_t +rcl_action_get_zero_initialized_endpoint_info_array(void); + +/// Finalize a rcl_action_endpoint_info_array_t. +/** + * The info_array struct has its members deallocated and reset to `NULL` or + * zero using the given allocator. + * + * \param[inout] info_array object to be finalized + * \param[in] allocator the allocator used to allocate the array and its contents + * \return `RCL_RET_OK` if successful, 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_endpoint_info_array_fini( + rcl_action_endpoint_info_array_t * info_array, + rcutils_allocator_t * allocator); + +/// Return a list of endpoint information for each action client of a given action. +/** + * The `node` parameter must point to a valid node. + * + * The `action_name` parameter must not be `NULL` and must be a fully + * qualified action name. + * The action name is not automatically remapped by this function. + * + * Each entry of the returned list aggregates the endpoint information of all + * the underlying entities of one action client, i.e. the clients of the + * goal, cancel, and result services and the subscriptions on the feedback + * and status topics. + * See rcl_action_endpoint_info_t for the correlation semantics. + * + * The `clients_info` parameter must be allocated and zero initialized with + * rcl_action_get_zero_initialized_endpoint_info_array(). + * This function allocates memory for the returned list of endpoint information + * and so it is the callers responsibility to pass `clients_info` to + * rcl_action_endpoint_info_array_fini() when it is no longer needed. + * Failing to do so will result in leaked memory. + * + *
+ * 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] allocator allocator for allocating space for the returned information + * \param[in] action_name the fully qualified name of the action in question + * \param[out] clients_info list of action client endpoint 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_ACTION_NAME_INVALID` if the action name is invalid, or + * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_get_clients_info_by_action( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * action_name, + rcl_action_endpoint_info_array_t * clients_info); + +/// Return a list of endpoint information for each action server of a given action. +/** + * The `node` parameter must point to a valid node. + * + * The `action_name` parameter must not be `NULL` and must be a fully + * qualified action name. + * The action name is not automatically remapped by this function. + * + * Each entry of the returned list aggregates the endpoint information of all + * the underlying entities of one action server, i.e. the servers of the + * goal, cancel, and result services and the publishers on the feedback and + * status topics. + * See rcl_action_endpoint_info_t for the correlation semantics. + * + * The `servers_info` parameter must be allocated and zero initialized with + * rcl_action_get_zero_initialized_endpoint_info_array(). + * This function allocates memory for the returned list of endpoint information + * and so it is the callers responsibility to pass `servers_info` to + * rcl_action_endpoint_info_array_fini() when it is no longer needed. + * Failing to do so will result in leaked memory. + * + *
+ * 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] allocator allocator for allocating space for the returned information + * \param[in] action_name the fully qualified name of the action in question + * \param[out] servers_info list of action server endpoint 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_ACTION_NAME_INVALID` if the action name is invalid, or + * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_get_servers_info_by_action( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * action_name, + rcl_action_endpoint_info_array_t * servers_info); + #ifdef __cplusplus } #endif diff --git a/rcl_action/src/rcl_action/graph.c b/rcl_action/src/rcl_action/graph.c index fce7be3bd..1f483e390 100644 --- a/rcl_action/src/rcl_action/graph.c +++ b/rcl_action/src/rcl_action/graph.c @@ -26,6 +26,7 @@ extern "C" #include "rcutils/strdup.h" #include "rcl_action/graph.h" +#include "rcl_action/names.h" static rcl_ret_t @@ -329,6 +330,321 @@ rcl_action_count_servers( return _rcl_action_count_entities(node, action_name, count, 0); } +rcl_action_endpoint_info_t +rcl_action_get_zero_initialized_endpoint_info(void) +{ + rcl_action_endpoint_info_t zero_info; + zero_info.goal_service_info = rmw_get_zero_initialized_service_endpoint_info(); + zero_info.cancel_service_info = rmw_get_zero_initialized_service_endpoint_info(); + zero_info.result_service_info = rmw_get_zero_initialized_service_endpoint_info(); + zero_info.feedback_topic_info = rmw_get_zero_initialized_topic_endpoint_info(); + zero_info.status_topic_info = rmw_get_zero_initialized_topic_endpoint_info(); + return zero_info; +} + +rcl_action_endpoint_info_array_t +rcl_action_get_zero_initialized_endpoint_info_array(void) +{ + const rcl_action_endpoint_info_array_t zero_array = {0u, NULL}; + return zero_array; +} + +rcl_ret_t +rcl_action_endpoint_info_array_fini( + rcl_action_endpoint_info_array_t * info_array, + rcutils_allocator_t * allocator) +{ + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(info_array, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = RCL_RET_OK; + for (size_t i = 0u; i < info_array->size; ++i) { + rcl_action_endpoint_info_t * info = &info_array->info_array[i]; + if (RMW_RET_OK != rmw_service_endpoint_info_fini(&info->goal_service_info, allocator)) { + ret = RCL_RET_ERROR; + } + if (RMW_RET_OK != rmw_service_endpoint_info_fini(&info->cancel_service_info, allocator)) { + ret = RCL_RET_ERROR; + } + if (RMW_RET_OK != rmw_service_endpoint_info_fini(&info->result_service_info, allocator)) { + ret = RCL_RET_ERROR; + } + if (RMW_RET_OK != rmw_topic_endpoint_info_fini(&info->feedback_topic_info, allocator)) { + ret = RCL_RET_ERROR; + } + if (RMW_RET_OK != rmw_topic_endpoint_info_fini(&info->status_topic_info, allocator)) { + ret = RCL_RET_ERROR; + } + } + if (NULL != info_array->info_array) { + allocator->deallocate(info_array->info_array, allocator->state); + } + info_array->info_array = NULL; + info_array->size = 0u; + return ret; +} + +/// Find the first not yet claimed service endpoint info matching the given node. +/** + * Claimed entries are recognized by a `NULL` node name (see the shallow move + * in _rcl_action_get_info_by_action below). + */ +static rmw_service_endpoint_info_t * +_find_service_endpoint_info_by_node( + rcl_service_endpoint_info_array_t * info_array, + const char * node_name, + const char * node_namespace) +{ + for (size_t i = 0u; i < info_array->size; ++i) { + rmw_service_endpoint_info_t * info = &info_array->info_array[i]; + if (NULL == info->node_name || NULL == info->node_namespace) { + continue; + } + if (0 == strcmp(info->node_name, node_name) && + 0 == strcmp(info->node_namespace, node_namespace)) + { + return info; + } + } + return NULL; +} + +/// Find the first not yet claimed topic endpoint info matching the given node. +static rmw_topic_endpoint_info_t * +_find_topic_endpoint_info_by_node( + rcl_topic_endpoint_info_array_t * info_array, + const char * node_name, + const char * node_namespace) +{ + for (size_t i = 0u; i < info_array->size; ++i) { + rmw_topic_endpoint_info_t * info = &info_array->info_array[i]; + if (NULL == info->node_name || NULL == info->node_namespace) { + continue; + } + if (0 == strcmp(info->node_name, node_name) && + 0 == strcmp(info->node_namespace, node_namespace)) + { + return info; + } + } + return NULL; +} + +static rcl_ret_t +_rcl_action_get_info_by_action( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * action_name, + rcl_action_endpoint_info_array_t * info_array, + int is_client) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(action_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(info_array, RCL_RET_INVALID_ARGUMENT); + if (0u != info_array->size || NULL != info_array->info_array) { + RCL_SET_ERROR_MSG( + "rcl_action_endpoint_info_array_t must be zero initialized, " + "use rcl_action_get_zero_initialized_endpoint_info_array"); + return RCL_RET_INVALID_ARGUMENT; + } + + char * goal_service_name = NULL; + char * cancel_service_name = NULL; + char * result_service_name = NULL; + char * feedback_topic_name = NULL; + char * status_topic_name = NULL; + + rcl_service_endpoint_info_array_t goal_infos = + rcl_get_zero_initialized_service_endpoint_info_array(); + rcl_service_endpoint_info_array_t cancel_infos = + rcl_get_zero_initialized_service_endpoint_info_array(); + rcl_service_endpoint_info_array_t result_infos = + rcl_get_zero_initialized_service_endpoint_info_array(); + rcl_topic_endpoint_info_array_t feedback_infos = + rmw_get_zero_initialized_topic_endpoint_info_array(); + rcl_topic_endpoint_info_array_t status_infos = + rmw_get_zero_initialized_topic_endpoint_info_array(); + + rcl_ret_t ret = rcl_action_get_goal_service_name(action_name, *allocator, &goal_service_name); + if (RCL_RET_OK == ret) { + ret = rcl_action_get_cancel_service_name(action_name, *allocator, &cancel_service_name); + } + if (RCL_RET_OK == ret) { + ret = rcl_action_get_result_service_name(action_name, *allocator, &result_service_name); + } + if (RCL_RET_OK == ret) { + ret = rcl_action_get_feedback_topic_name(action_name, *allocator, &feedback_topic_name); + } + if (RCL_RET_OK == ret) { + ret = rcl_action_get_status_topic_name(action_name, *allocator, &status_topic_name); + } + + // Query the endpoint information of all the underlying entities of the action. + // Action clients are clients of the services and subscriptions on the topics, + // while action servers are servers of the services and publishers on the topics. + if (RCL_RET_OK == ret) { + if (is_client) { + ret = rcl_get_clients_info_by_service( + node, allocator, goal_service_name, false, &goal_infos); + } else { + ret = rcl_get_servers_info_by_service( + node, allocator, goal_service_name, false, &goal_infos); + } + } + if (RCL_RET_OK == ret) { + if (is_client) { + ret = rcl_get_clients_info_by_service( + node, allocator, cancel_service_name, false, &cancel_infos); + } else { + ret = rcl_get_servers_info_by_service( + node, allocator, cancel_service_name, false, &cancel_infos); + } + } + if (RCL_RET_OK == ret) { + if (is_client) { + ret = rcl_get_clients_info_by_service( + node, allocator, result_service_name, false, &result_infos); + } else { + ret = rcl_get_servers_info_by_service( + node, allocator, result_service_name, false, &result_infos); + } + } + if (RCL_RET_OK == ret) { + if (is_client) { + ret = rcl_get_subscriptions_info_by_topic( + node, allocator, feedback_topic_name, false, &feedback_infos); + } else { + ret = rcl_get_publishers_info_by_topic( + node, allocator, feedback_topic_name, false, &feedback_infos); + } + } + if (RCL_RET_OK == ret) { + if (is_client) { + ret = rcl_get_subscriptions_info_by_topic( + node, allocator, status_topic_name, false, &status_infos); + } else { + ret = rcl_get_publishers_info_by_topic( + node, allocator, status_topic_name, false, &status_infos); + } + } + + // The goal service endpoint is the canonical identity of an action client + // or an action server, correlate the other entities to it by node name and + // node namespace. + if (RCL_RET_OK == ret && goal_infos.size > 0u) { + info_array->info_array = allocator->allocate( + sizeof(rcl_action_endpoint_info_t) * goal_infos.size, allocator->state); + if (NULL == info_array->info_array) { + RCL_SET_ERROR_MSG("Failed to allocate memory for action endpoint info array"); + ret = RCL_RET_BAD_ALLOC; + } else { + info_array->size = goal_infos.size; + for (size_t i = 0u; i < info_array->size; ++i) { + rcl_action_endpoint_info_t * endpoint_info = &info_array->info_array[i]; + *endpoint_info = rcl_action_get_zero_initialized_endpoint_info(); + // Shallow move: transfer ownership of the allocated members and + // zero initialize the source so that finalizing the source array + // does not free them. + endpoint_info->goal_service_info = goal_infos.info_array[i]; + goal_infos.info_array[i] = rmw_get_zero_initialized_service_endpoint_info(); + + const char * node_name = endpoint_info->goal_service_info.node_name; + const char * node_namespace = endpoint_info->goal_service_info.node_namespace; + if (NULL == node_name || NULL == node_namespace) { + continue; + } + rmw_service_endpoint_info_t * service_info = + _find_service_endpoint_info_by_node(&cancel_infos, node_name, node_namespace); + if (NULL != service_info) { + endpoint_info->cancel_service_info = *service_info; + *service_info = rmw_get_zero_initialized_service_endpoint_info(); + } + service_info = + _find_service_endpoint_info_by_node(&result_infos, node_name, node_namespace); + if (NULL != service_info) { + endpoint_info->result_service_info = *service_info; + *service_info = rmw_get_zero_initialized_service_endpoint_info(); + } + rmw_topic_endpoint_info_t * topic_info = + _find_topic_endpoint_info_by_node(&feedback_infos, node_name, node_namespace); + if (NULL != topic_info) { + endpoint_info->feedback_topic_info = *topic_info; + *topic_info = rmw_get_zero_initialized_topic_endpoint_info(); + } + topic_info = + _find_topic_endpoint_info_by_node(&status_infos, node_name, node_namespace); + if (NULL != topic_info) { + endpoint_info->status_topic_info = *topic_info; + *topic_info = rmw_get_zero_initialized_topic_endpoint_info(); + } + } + } + } + + // Cleanup the intermediate arrays and names. + // Entries claimed above were zero initialized in the source arrays, so + // finalizing the source arrays only frees the unclaimed entries. + if (RMW_RET_OK != rmw_service_endpoint_info_array_fini(&goal_infos, allocator) || + RMW_RET_OK != rmw_service_endpoint_info_array_fini(&cancel_infos, allocator) || + RMW_RET_OK != rmw_service_endpoint_info_array_fini(&result_infos, allocator) || + RMW_RET_OK != rmw_topic_endpoint_info_array_fini(&feedback_infos, allocator) || + RMW_RET_OK != rmw_topic_endpoint_info_array_fini(&status_infos, allocator)) + { + if (RCL_RET_OK == ret) { + ret = RCL_RET_ERROR; + } + } + if (NULL != goal_service_name) { + allocator->deallocate(goal_service_name, allocator->state); + } + if (NULL != cancel_service_name) { + allocator->deallocate(cancel_service_name, allocator->state); + } + if (NULL != result_service_name) { + allocator->deallocate(result_service_name, allocator->state); + } + if (NULL != feedback_topic_name) { + allocator->deallocate(feedback_topic_name, allocator->state); + } + if (NULL != status_topic_name) { + allocator->deallocate(status_topic_name, allocator->state); + } + + if (RCL_RET_OK != ret) { + rcl_ret_t fini_ret = rcl_action_endpoint_info_array_fini(info_array, allocator); + if (RCL_RET_OK != fini_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Freeing action endpoint info array failed while handling a previous error. " + "Leaking memory!\n"); + } + } + + return ret; +} + +rcl_ret_t +rcl_action_get_clients_info_by_action( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * action_name, + rcl_action_endpoint_info_array_t * clients_info) +{ + return _rcl_action_get_info_by_action(node, allocator, action_name, clients_info, 1); +} + +rcl_ret_t +rcl_action_get_servers_info_by_action( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * action_name, + rcl_action_endpoint_info_array_t * servers_info) +{ + return _rcl_action_get_info_by_action(node, allocator, action_name, servers_info, 0); +} + #ifdef __cplusplus } #endif diff --git a/rcl_action/test/rcl_action/test_graph.cpp b/rcl_action/test/rcl_action/test_graph.cpp index bd797e49f..ac75da69a 100644 --- a/rcl_action/test/rcl_action/test_graph.cpp +++ b/rcl_action/test/rcl_action/test_graph.cpp @@ -382,6 +382,50 @@ class TestActionGraphMultiNodeFixture : public TestActionGraphFixture curr_time = std::chrono::system_clock::now(); } } + + using GetInfoByActionFunc = rcl_ret_t (*)( + const rcl_node_t *, + rcutils_allocator_t *, + const char *, + rcl_action_endpoint_info_array_t *); + + static bool endpoint_info_is_complete(const rcl_action_endpoint_info_t * info) + { + return NULL != info->goal_service_info.node_name && + NULL != info->cancel_service_info.node_name && + NULL != info->result_service_info.node_name && + NULL != info->feedback_topic_info.node_name && + NULL != info->status_topic_info.node_name; + } + + void wait_for_action_info_count( + GetInfoByActionFunc func, + size_t expected_count, + std::chrono::milliseconds duration) + { + auto start_time = std::chrono::system_clock::now(); + auto curr_time = start_time; + + rcl_ret_t ret; + while ((curr_time - start_time) < duration) { + rcl_action_endpoint_info_array_t info_array = + rcl_action_get_zero_initialized_endpoint_info_array(); + ret = func(&this->node, &this->allocator, this->action_name, &info_array); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + size_t info_count = info_array.size; + bool complete = true; + for (size_t i = 0u; i < info_array.size; ++i) { + complete &= endpoint_info_is_complete(&info_array.info_array[i]); + } + EXPECT_EQ( + RCL_RET_OK, rcl_action_endpoint_info_array_fini(&info_array, &this->allocator)); + if (info_count == expected_count && complete) { + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + curr_time = std::chrono::system_clock::now(); + } + } }; // Note, this test could be affected by other communication on the same ROS domain @@ -857,3 +901,227 @@ TEST_F(TestActionGraphMultiNodeFixture, test_action_count_servers) EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_EQ(1u, count); } + +TEST_F(TestActionGraphFixture, test_action_get_clients_servers_info_by_action) +{ + rcl_ret_t ret; + const char * test_action_name = "/test_action"; + rcl_action_endpoint_info_array_t info_array = + rcl_action_get_zero_initialized_endpoint_info_array(); + + // Invalid node + ret = rcl_action_get_clients_info_by_action( + nullptr, &this->allocator, test_action_name, &info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_clients_info_by_action( + &this->zero_node, &this->allocator, test_action_name, &info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_servers_info_by_action( + &this->old_node, &this->allocator, test_action_name, &info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid allocator + ret = rcl_action_get_clients_info_by_action( + &this->node, nullptr, test_action_name, &info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_servers_info_by_action( + &this->node, &this->zero_allocator, test_action_name, &info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid action name + ret = rcl_action_get_clients_info_by_action( + &this->node, &this->allocator, nullptr, &info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_servers_info_by_action( + &this->node, &this->allocator, "", &info_array); + EXPECT_EQ(RCL_RET_ACTION_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid info array + ret = rcl_action_get_clients_info_by_action( + &this->node, &this->allocator, test_action_name, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + rcl_action_endpoint_info_t info = rcl_action_get_zero_initialized_endpoint_info(); + info_array.size = 1u; + info_array.info_array = &info; + ret = rcl_action_get_servers_info_by_action( + &this->node, &this->allocator, test_action_name, &info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + info_array = rcl_action_get_zero_initialized_endpoint_info_array(); + + // Valid calls, no endpoints + ret = rcl_action_get_clients_info_by_action( + &this->node, &this->allocator, test_action_name, &info_array); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0u, info_array.size); + EXPECT_EQ( + RCL_RET_OK, rcl_action_endpoint_info_array_fini(&info_array, &this->allocator)); + info_array = rcl_action_get_zero_initialized_endpoint_info_array(); + ret = rcl_action_get_servers_info_by_action( + &this->node, &this->allocator, test_action_name, &info_array); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0u, info_array.size); + EXPECT_EQ( + RCL_RET_OK, rcl_action_endpoint_info_array_fini(&info_array, &this->allocator)); +} + +// Note, this test could be affected by other communication on the same ROS domain +TEST_F(TestActionGraphMultiNodeFixture, test_action_get_clients_info_by_action) +{ + rcl_ret_t ret; + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + rcl_action_client_t action_client = rcl_action_get_zero_initialized_client(); + rcl_action_client_options_t action_client_options = rcl_action_client_get_default_options(); + ret = rcl_action_client_init( + &action_client, + &this->remote_node, + action_typesupport, + this->action_name, + &action_client_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_action_client_fini(&action_client, &this->remote_node)) << + rcl_get_error_string().str; + }); + + // Wait for all the underlying entities of the action client to appear in the graph + wait_for_action_count(clients_by_node_func, 1u, std::chrono::seconds(1)); + wait_for_action_info_count( + rcl_action_get_clients_info_by_action, 1u, std::chrono::seconds(5)); + + rcl_action_endpoint_info_array_t info_array = + rcl_action_get_zero_initialized_endpoint_info_array(); + ret = rcl_action_get_clients_info_by_action( + &this->node, &this->allocator, this->action_name, &info_array); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(1u, info_array.size); + const rcl_action_endpoint_info_t * endpoint_info = &info_array.info_array[0]; + + // The goal service endpoint is the canonical identity of the action client + const rmw_service_endpoint_info_t * goal_info = &endpoint_info->goal_service_info; + EXPECT_STREQ(this->remote_node_name, goal_info->node_name); + EXPECT_STREQ("/", goal_info->node_namespace); + EXPECT_STREQ("test_msgs/action/Fibonacci_SendGoal", goal_info->service_type); + EXPECT_EQ(RMW_ENDPOINT_CLIENT, goal_info->endpoint_type); + EXPECT_TRUE(1u == goal_info->endpoint_count || 2u == goal_info->endpoint_count); + + // The other underlying entities are correlated by node name and namespace + const rmw_service_endpoint_info_t * cancel_info = &endpoint_info->cancel_service_info; + EXPECT_STREQ(this->remote_node_name, cancel_info->node_name); + EXPECT_STREQ("action_msgs/srv/CancelGoal", cancel_info->service_type); + EXPECT_EQ(RMW_ENDPOINT_CLIENT, cancel_info->endpoint_type); + const rmw_service_endpoint_info_t * result_info = &endpoint_info->result_service_info; + EXPECT_STREQ(this->remote_node_name, result_info->node_name); + EXPECT_STREQ("test_msgs/action/Fibonacci_GetResult", result_info->service_type); + EXPECT_EQ(RMW_ENDPOINT_CLIENT, result_info->endpoint_type); + const rmw_topic_endpoint_info_t * feedback_info = &endpoint_info->feedback_topic_info; + EXPECT_STREQ(this->remote_node_name, feedback_info->node_name); + EXPECT_STREQ("test_msgs/action/Fibonacci_FeedbackMessage", feedback_info->topic_type); + EXPECT_EQ(RMW_ENDPOINT_SUBSCRIPTION, feedback_info->endpoint_type); + const rmw_topic_endpoint_info_t * status_info = &endpoint_info->status_topic_info; + EXPECT_STREQ(this->remote_node_name, status_info->node_name); + EXPECT_STREQ("action_msgs/msg/GoalStatusArray", status_info->topic_type); + EXPECT_EQ(RMW_ENDPOINT_SUBSCRIPTION, status_info->endpoint_type); + + // There should be no action servers + rcl_action_endpoint_info_array_t server_info_array = + rcl_action_get_zero_initialized_endpoint_info_array(); + ret = rcl_action_get_servers_info_by_action( + &this->node, &this->allocator, this->action_name, &server_info_array); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0u, server_info_array.size); + + EXPECT_EQ( + RCL_RET_OK, rcl_action_endpoint_info_array_fini(&info_array, &this->allocator)); + EXPECT_EQ( + RCL_RET_OK, rcl_action_endpoint_info_array_fini(&server_info_array, &this->allocator)); +} + +// Note, this test could be affected by other communication on the same ROS domain +TEST_F(TestActionGraphMultiNodeFixture, test_action_get_servers_info_by_action) +{ + rcl_ret_t ret; + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + rcl_action_server_t action_server = rcl_action_get_zero_initialized_server(); + rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &this->allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + rcl_action_server_options_t action_server_options = rcl_action_server_get_default_options(); + ret = rcl_action_server_init( + &action_server, + &this->remote_node, + &clock, + action_typesupport, + this->action_name, + &action_server_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_action_server_fini(&action_server, &this->remote_node)) << + rcl_get_error_string().str; + }); + + // Wait for all the underlying entities of the action server to appear in the graph + wait_for_action_count(servers_by_node_func, 1u, std::chrono::seconds(1)); + wait_for_action_info_count( + rcl_action_get_servers_info_by_action, 1u, std::chrono::seconds(5)); + + rcl_action_endpoint_info_array_t info_array = + rcl_action_get_zero_initialized_endpoint_info_array(); + ret = rcl_action_get_servers_info_by_action( + &this->node, &this->allocator, this->action_name, &info_array); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(1u, info_array.size); + const rcl_action_endpoint_info_t * endpoint_info = &info_array.info_array[0]; + + // The goal service endpoint is the canonical identity of the action server + const rmw_service_endpoint_info_t * goal_info = &endpoint_info->goal_service_info; + EXPECT_STREQ(this->remote_node_name, goal_info->node_name); + EXPECT_STREQ("/", goal_info->node_namespace); + EXPECT_STREQ("test_msgs/action/Fibonacci_SendGoal", goal_info->service_type); + EXPECT_EQ(RMW_ENDPOINT_SERVER, goal_info->endpoint_type); + EXPECT_TRUE(1u == goal_info->endpoint_count || 2u == goal_info->endpoint_count); + + // The other underlying entities are correlated by node name and namespace + const rmw_service_endpoint_info_t * cancel_info = &endpoint_info->cancel_service_info; + EXPECT_STREQ(this->remote_node_name, cancel_info->node_name); + EXPECT_STREQ("action_msgs/srv/CancelGoal", cancel_info->service_type); + EXPECT_EQ(RMW_ENDPOINT_SERVER, cancel_info->endpoint_type); + const rmw_service_endpoint_info_t * result_info = &endpoint_info->result_service_info; + EXPECT_STREQ(this->remote_node_name, result_info->node_name); + EXPECT_STREQ("test_msgs/action/Fibonacci_GetResult", result_info->service_type); + EXPECT_EQ(RMW_ENDPOINT_SERVER, result_info->endpoint_type); + const rmw_topic_endpoint_info_t * feedback_info = &endpoint_info->feedback_topic_info; + EXPECT_STREQ(this->remote_node_name, feedback_info->node_name); + EXPECT_STREQ("test_msgs/action/Fibonacci_FeedbackMessage", feedback_info->topic_type); + EXPECT_EQ(RMW_ENDPOINT_PUBLISHER, feedback_info->endpoint_type); + const rmw_topic_endpoint_info_t * status_info = &endpoint_info->status_topic_info; + EXPECT_STREQ(this->remote_node_name, status_info->node_name); + EXPECT_STREQ("action_msgs/msg/GoalStatusArray", status_info->topic_type); + EXPECT_EQ(RMW_ENDPOINT_PUBLISHER, status_info->endpoint_type); + + // There should be no action clients + rcl_action_endpoint_info_array_t client_info_array = + rcl_action_get_zero_initialized_endpoint_info_array(); + ret = rcl_action_get_clients_info_by_action( + &this->node, &this->allocator, this->action_name, &client_info_array); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0u, client_info_array.size); + + EXPECT_EQ( + RCL_RET_OK, rcl_action_endpoint_info_array_fini(&info_array, &this->allocator)); + EXPECT_EQ( + RCL_RET_OK, rcl_action_endpoint_info_array_fini(&client_info_array, &this->allocator)); +} From afa078b97cd93e9c06738d33d56b10f5c41ee714 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 20 Jul 2026 08:36:21 +0900 Subject: [PATCH 2/4] add defensive code to check size and array mismatch. Signed-off-by: Tomoya Fujita --- rcl_action/src/rcl_action/graph.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rcl_action/src/rcl_action/graph.c b/rcl_action/src/rcl_action/graph.c index 1f483e390..33141b8ac 100644 --- a/rcl_action/src/rcl_action/graph.c +++ b/rcl_action/src/rcl_action/graph.c @@ -356,6 +356,10 @@ rcl_action_endpoint_info_array_fini( { RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(info_array, RCL_RET_INVALID_ARGUMENT); + if (info_array->size > 0u && NULL == info_array->info_array) { + RCL_SET_ERROR_MSG("info_array->info_array is NULL but size is non-zero"); + return RCL_RET_INVALID_ARGUMENT; + } rcl_ret_t ret = RCL_RET_OK; for (size_t i = 0u; i < info_array->size; ++i) { From af23f31d25b2dfb9c05fcae8691da3fefa058e7b Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 22 Jul 2026 09:15:58 +0900 Subject: [PATCH 3/4] introduce helper function to get endpoint name and infos. Signed-off-by: Tomoya Fujita --- rcl_action/src/rcl_action/graph.c | 149 +++++++++++++++--------------- 1 file changed, 73 insertions(+), 76 deletions(-) diff --git a/rcl_action/src/rcl_action/graph.c b/rcl_action/src/rcl_action/graph.c index 33141b8ac..c9c8c8d82 100644 --- a/rcl_action/src/rcl_action/graph.c +++ b/rcl_action/src/rcl_action/graph.c @@ -434,6 +434,63 @@ _find_topic_endpoint_info_by_node( return NULL; } +typedef rcl_ret_t (* rcl_action_get_name_func_t)( + const char * action_name, + rcl_allocator_t allocator, + char ** name); + +/// Resolve the name of an underlying action service and query its endpoint info. +static rcl_ret_t +_rcl_action_get_service_info( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * action_name, + rcl_action_get_name_func_t get_service_name, + int is_client, + rcl_service_endpoint_info_array_t * infos) +{ + char * service_name = NULL; + rcl_ret_t ret = get_service_name(action_name, *allocator, &service_name); + if (RCL_RET_OK != ret) { + return ret; + } + // Action clients are clients of the underlying services, + // while action servers are servers of the underlying services. + if (is_client) { + ret = rcl_get_clients_info_by_service(node, allocator, service_name, false, infos); + } else { + ret = rcl_get_servers_info_by_service(node, allocator, service_name, false, infos); + } + allocator->deallocate(service_name, allocator->state); + return ret; +} + +/// Resolve the name of an underlying action topic and query its endpoint info. +static rcl_ret_t +_rcl_action_get_topic_info( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * action_name, + rcl_action_get_name_func_t get_topic_name, + int is_client, + rcl_topic_endpoint_info_array_t * infos) +{ + char * topic_name = NULL; + rcl_ret_t ret = get_topic_name(action_name, *allocator, &topic_name); + if (RCL_RET_OK != ret) { + return ret; + } + // Action clients are subscriptions on the underlying topics, + // while action servers are publishers on the underlying topics. + if (is_client) { + ret = rcl_get_subscriptions_info_by_topic(node, allocator, topic_name, false, infos); + } else { + ret = rcl_get_publishers_info_by_topic(node, allocator, topic_name, false, infos); + } + allocator->deallocate(topic_name, allocator->state); + return ret; +} + static rcl_ret_t _rcl_action_get_info_by_action( const rcl_node_t * node, @@ -455,12 +512,6 @@ _rcl_action_get_info_by_action( return RCL_RET_INVALID_ARGUMENT; } - char * goal_service_name = NULL; - char * cancel_service_name = NULL; - char * result_service_name = NULL; - char * feedback_topic_name = NULL; - char * status_topic_name = NULL; - rcl_service_endpoint_info_array_t goal_infos = rcl_get_zero_initialized_service_endpoint_info_array(); rcl_service_endpoint_info_array_t cancel_infos = @@ -472,67 +523,29 @@ _rcl_action_get_info_by_action( rcl_topic_endpoint_info_array_t status_infos = rmw_get_zero_initialized_topic_endpoint_info_array(); - rcl_ret_t ret = rcl_action_get_goal_service_name(action_name, *allocator, &goal_service_name); - if (RCL_RET_OK == ret) { - ret = rcl_action_get_cancel_service_name(action_name, *allocator, &cancel_service_name); - } - if (RCL_RET_OK == ret) { - ret = rcl_action_get_result_service_name(action_name, *allocator, &result_service_name); - } - if (RCL_RET_OK == ret) { - ret = rcl_action_get_feedback_topic_name(action_name, *allocator, &feedback_topic_name); - } - if (RCL_RET_OK == ret) { - ret = rcl_action_get_status_topic_name(action_name, *allocator, &status_topic_name); - } - // Query the endpoint information of all the underlying entities of the action. - // Action clients are clients of the services and subscriptions on the topics, - // while action servers are servers of the services and publishers on the topics. + rcl_ret_t ret = _rcl_action_get_service_info( + node, allocator, action_name, + rcl_action_get_goal_service_name, is_client, &goal_infos); if (RCL_RET_OK == ret) { - if (is_client) { - ret = rcl_get_clients_info_by_service( - node, allocator, goal_service_name, false, &goal_infos); - } else { - ret = rcl_get_servers_info_by_service( - node, allocator, goal_service_name, false, &goal_infos); - } + ret = _rcl_action_get_service_info( + node, allocator, action_name, + rcl_action_get_cancel_service_name, is_client, &cancel_infos); } if (RCL_RET_OK == ret) { - if (is_client) { - ret = rcl_get_clients_info_by_service( - node, allocator, cancel_service_name, false, &cancel_infos); - } else { - ret = rcl_get_servers_info_by_service( - node, allocator, cancel_service_name, false, &cancel_infos); - } + ret = _rcl_action_get_service_info( + node, allocator, action_name, + rcl_action_get_result_service_name, is_client, &result_infos); } if (RCL_RET_OK == ret) { - if (is_client) { - ret = rcl_get_clients_info_by_service( - node, allocator, result_service_name, false, &result_infos); - } else { - ret = rcl_get_servers_info_by_service( - node, allocator, result_service_name, false, &result_infos); - } + ret = _rcl_action_get_topic_info( + node, allocator, action_name, + rcl_action_get_feedback_topic_name, is_client, &feedback_infos); } if (RCL_RET_OK == ret) { - if (is_client) { - ret = rcl_get_subscriptions_info_by_topic( - node, allocator, feedback_topic_name, false, &feedback_infos); - } else { - ret = rcl_get_publishers_info_by_topic( - node, allocator, feedback_topic_name, false, &feedback_infos); - } - } - if (RCL_RET_OK == ret) { - if (is_client) { - ret = rcl_get_subscriptions_info_by_topic( - node, allocator, status_topic_name, false, &status_infos); - } else { - ret = rcl_get_publishers_info_by_topic( - node, allocator, status_topic_name, false, &status_infos); - } + ret = _rcl_action_get_topic_info( + node, allocator, action_name, + rcl_action_get_status_topic_name, is_client, &status_infos); } // The goal service endpoint is the canonical identity of an action client @@ -588,7 +601,7 @@ _rcl_action_get_info_by_action( } } - // Cleanup the intermediate arrays and names. + // Cleanup the intermediate arrays. // Entries claimed above were zero initialized in the source arrays, so // finalizing the source arrays only frees the unclaimed entries. if (RMW_RET_OK != rmw_service_endpoint_info_array_fini(&goal_infos, allocator) || @@ -601,22 +614,6 @@ _rcl_action_get_info_by_action( ret = RCL_RET_ERROR; } } - if (NULL != goal_service_name) { - allocator->deallocate(goal_service_name, allocator->state); - } - if (NULL != cancel_service_name) { - allocator->deallocate(cancel_service_name, allocator->state); - } - if (NULL != result_service_name) { - allocator->deallocate(result_service_name, allocator->state); - } - if (NULL != feedback_topic_name) { - allocator->deallocate(feedback_topic_name, allocator->state); - } - if (NULL != status_topic_name) { - allocator->deallocate(status_topic_name, allocator->state); - } - if (RCL_RET_OK != ret) { rcl_ret_t fini_ret = rcl_action_endpoint_info_array_fini(info_array, allocator); if (RCL_RET_OK != fini_ret) { From 89be6235a1d965cb5a5e6e10182a3d004a9a7d07 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 30 Jul 2026 16:41:48 +0900 Subject: [PATCH 4/4] Count entities on the action's goal service instead. Signed-off-by: Tomoya Fujita --- rcl_action/include/rcl_action/graph.h | 18 +++++--- rcl_action/src/rcl_action/graph.c | 63 ++++++--------------------- 2 files changed, 25 insertions(+), 56 deletions(-) diff --git a/rcl_action/include/rcl_action/graph.h b/rcl_action/include/rcl_action/graph.h index d8bba4ffc..f08b3e202 100644 --- a/rcl_action/include/rcl_action/graph.h +++ b/rcl_action/include/rcl_action/graph.h @@ -164,10 +164,13 @@ rcl_action_get_names_and_types( * The `count` parameter must not be `NULL`. * The `count` parameter is the output for this function and will be set. * - * This function counts the number of action clients for the given action name - * across all nodes known in the ROS graph. + * This function counts the number of action client instances for the given + * action name across all nodes known in the ROS graph. + * Each action client is counted individually, so a node with two action + * clients on the same action contributes a count of 2. * - * The action name is not automatically remapped by this function. + * The `action_name` parameter should be a fully qualified action name, since + * it is not expanded or automatically remapped by this function. * If there is a client created with action name `foo` and remap rule `foo:=bar` * then calling this with `action_name` set to `bar` will return a count of 1, * and with `action_name` set to `foo` will return a count of 0. @@ -206,10 +209,13 @@ rcl_action_count_clients( * The `count` parameter must not be `NULL`. * The `count` parameter is the output for this function and will be set. * - * This function counts the number of action servers for the given action name - * across all nodes known in the ROS graph. + * This function counts the number of action server instances for the given + * action name across all nodes known in the ROS graph. + * Each action server is counted individually, so a node with two action + * servers on the same action contributes a count of 2. * - * The action name is not automatically remapped by this function. + * The `action_name` parameter should be a fully qualified action name, since + * it is not expanded or automatically remapped by this function. * If there is a server created with action name `foo` and remap rule `foo:=bar` * then calling this with `action_name` set to `bar` will return a count of 1, * and with `action_name` set to `foo` will return a count of 0. diff --git a/rcl_action/src/rcl_action/graph.c b/rcl_action/src/rcl_action/graph.c index c9c8c8d82..4a9203a85 100644 --- a/rcl_action/src/rcl_action/graph.c +++ b/rcl_action/src/rcl_action/graph.c @@ -252,63 +252,26 @@ _rcl_action_count_entities( *count = 0u; + // Count entities on the action's goal service instead of walking the list + // of discovered nodes: every action client has exactly one goal service + // client and every action server has exactly one goal service. + // Name-based per-node queries double-count when the graph momentarily + // holds duplicate node names (e.g. a stale instance of a restarted node). rcl_allocator_t allocator = rcl_get_default_allocator(); - rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); - rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); - - rcl_ret_t ret = rcl_get_node_names(node, allocator, &node_names, &node_namespaces); + char * goal_service_name = NULL; + rcl_ret_t ret = rcl_action_get_goal_service_name( + action_name, allocator, &goal_service_name); if (RCL_RET_OK != ret) { return ret; } - for (size_t i = 0u; i < node_names.size && RCL_RET_OK == ret; ++i) { - rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); - rcl_ret_t get_ret; - if (is_client) { - get_ret = rcl_action_get_client_names_and_types_by_node( - node, &allocator, node_names.data[i], node_namespaces.data[i], &nat); - } else { - get_ret = rcl_action_get_server_names_and_types_by_node( - node, &allocator, node_names.data[i], node_namespaces.data[i], &nat); - } - if (RCL_RET_NODE_NAME_NON_EXISTENT == get_ret) { - // Node may have disappeared between listing and querying, skip it - rcl_reset_error(); - continue; - } - if (RCL_RET_OK != get_ret) { - ret = get_ret; - break; - } - - for (size_t j = 0u; j < nat.names.size; ++j) { - if (strcmp(nat.names.data[j], action_name) == 0) { - ++(*count); - } - } - - rcl_ret_t fini_ret = rcl_names_and_types_fini(&nat); - if (RCL_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Freeing action names and types failed while counting. Leaking memory!\n"); - } - } - - rcutils_ret_t rcutils_ret = rcutils_string_array_fini(&node_names); - if (RCUTILS_RET_OK != rcutils_ret) { - if (RCL_RET_OK == ret) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - ret = RCL_RET_ERROR; - } - } - rcutils_ret = rcutils_string_array_fini(&node_namespaces); - if (RCUTILS_RET_OK != rcutils_ret) { - if (RCL_RET_OK == ret) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - ret = RCL_RET_ERROR; - } + if (is_client) { + ret = rcl_count_clients(node, goal_service_name, count); + } else { + ret = rcl_count_services(node, goal_service_name, count); } + allocator.deallocate(goal_service_name, allocator.state); return ret; }