From 9b89c9cd35d2c09dc1e90beedd7a6a1af0c4dd76 Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Tue, 25 Jun 2024 21:23:11 +0900 Subject: [PATCH 1/2] add : get clients, servers info Signed-off-by: Minju, Lee --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 226 ++++++++++++++++++++++++++++ 1 file changed, 226 insertions(+) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index af641101..4da38868 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -5837,6 +5837,232 @@ extern "C" rmw_ret_t rmw_get_subscriptions_info_by_topic( subscriptions_info); } +extern "C" rmw_ret_t rmw_get_clients_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * clients_info) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, node->implementation_identifier, eclipse_cyclonedds_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + if (RMW_RET_OK != rmw_topic_endpoint_info_array_check_zero(clients_info)) { + return RMW_RET_INVALID_ARGUMENT; + } + auto common_context = &node->context->impl->common; + std::string mangled_rq_topic_name, mangled_rp_topic_name; + mangled_rq_topic_name = mangled_rp_topic_name = service_name; + DemangleFunction demangle_type = _identity_demangle; + if (!no_mangle) { + mangled_rq_topic_name = \ + make_fqtopic(ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); + mangled_rp_topic_name = \ + make_fqtopic(ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false); + demangle_type = _demangle_if_ros_type; + } + + rmw_topic_endpoint_info_array_t publishers_info = \ + rmw_get_zero_initialized_topic_endpoint_info_array(); + rmw_ret_t ret = common_context->graph_cache.get_writers_info_by_topic( + mangled_rq_topic_name, + demangle_type, + allocator, + &publishers_info); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + publishers_info_delete_on_error( + &publishers_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy publishers_info when function failed."); + } + } + ); + if (RMW_RET_OK != ret) { + return ret; + } + + rmw_topic_endpoint_info_array_t subscriptions_info = \ + rmw_get_zero_initialized_topic_endpoint_info_array(); + ret = common_context->graph_cache.get_readers_info_by_topic( + mangled_rp_topic_name, + demangle_type, + allocator, + &subscriptions_info); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + subscriptions_info_delete_on_error( + &subscriptions_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy subscriptions_info when function failed."); + } + } + ); + if (RMW_RET_OK != ret) { + return ret; + } + + size_t total_size = publishers_info.size + subscriptions_info.size; + ret = rmw_topic_endpoint_info_array_init_with_size(clients_info, total_size, allocator); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + clients_info_delete_on_error( + clients_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy clients_info when function failed."); + } + } + ); + if (RMW_RET_OK != ret) { + return ret; + } + for (size_t i = 0; i < publishers_info.size; ++i) { + clients_info->info_array[i] = publishers_info.info_array[i]; + } + for (size_t i = 0; i < subscriptions_info.size; ++i) { + clients_info->info_array[publishers_info.size + i] = subscriptions_info.info_array[i]; + } + publishers_info_delete_on_error.release(); + subscriptions_info_delete_on_error.release(); + clients_info_delete_on_error.release(); + return RMW_RET_OK; +} + +extern "C" rmw_ret_t rmw_get_servers_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * servers_info) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, node->implementation_identifier, eclipse_cyclonedds_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + if (RMW_RET_OK != rmw_topic_endpoint_info_array_check_zero(servers_info)) { + return RMW_RET_INVALID_ARGUMENT; + } + + auto common_context = &node->context->impl->common; + + std::string mangled_rq_topic_name, mangled_rp_topic_name; + mangled_rq_topic_name = mangled_rp_topic_name = service_name; + DemangleFunction demangle_type = _identity_demangle; + if (!no_mangle) { + mangled_rq_topic_name = \ + make_fqtopic(ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); + mangled_rp_topic_name = \ + make_fqtopic(ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false); + demangle_type = _demangle_if_ros_type; + } + rmw_topic_endpoint_info_array_t subscriptions_info = \ + rmw_get_zero_initialized_topic_endpoint_info_array(); + rmw_ret_t ret = common_context->graph_cache.get_readers_info_by_topic( + mangled_rq_topic_name, + demangle_type, + allocator, + &subscriptions_info); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + subscriptions_info_delete_on_error( + &subscriptions_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy subscriptions_info when function failed."); + } + } + ); + if (RMW_RET_OK != ret) { + return ret; + } + rmw_topic_endpoint_info_array_t publishers_info = \ + rmw_get_zero_initialized_topic_endpoint_info_array(); + ret = common_context->graph_cache.get_writers_info_by_topic( + mangled_rp_topic_name, + demangle_type, + allocator, + &publishers_info); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + publishers_info_delete_on_error( + &publishers_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy publishers_info when function failed."); + } + } + ); + if (RMW_RET_OK != ret) { + return ret; + } + + size_t total_size = publishers_info.size + subscriptions_info.size; + ret = rmw_topic_endpoint_info_array_init_with_size(servers_info, total_size, allocator); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + servers_info_delete_on_error( + servers_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy servers_info when function failed."); + } + } + ); + if (RMW_RET_OK != ret) { + return ret; + } + for (size_t i = 0; i < publishers_info.size; ++i) { + servers_info->info_array[i] = publishers_info.info_array[i]; + } + for (size_t i = 0; i < subscriptions_info.size; ++i) { + servers_info->info_array[publishers_info.size + i] = subscriptions_info.info_array[i]; + } + publishers_info_delete_on_error.release(); + subscriptions_info_delete_on_error.release(); + servers_info_delete_on_error.release(); + return RMW_RET_OK; +} + extern "C" rmw_ret_t rmw_qos_profile_check_compatible( const rmw_qos_profile_t publisher_profile, const rmw_qos_profile_t subscription_profile, From 9754467ceddc51d4a6b035ed2f859ea61c137c8a Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Fri, 27 Sep 2024 22:34:29 +0900 Subject: [PATCH 2/2] Fixes service_info Signed-off-by: Minju, Lee --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 172 +--------------------------- 1 file changed, 6 insertions(+), 166 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 4da38868..79e89071 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -5855,99 +5855,18 @@ extern "C" rmw_ret_t rmw_get_clients_info_by_service( return RMW_RET_INVALID_ARGUMENT; } auto common_context = &node->context->impl->common; - std::string mangled_rq_topic_name, mangled_rp_topic_name; - mangled_rq_topic_name = mangled_rp_topic_name = service_name; + std::string mangled_rp_topic_name = service_name; DemangleFunction demangle_type = _identity_demangle; if (!no_mangle) { - mangled_rq_topic_name = \ - make_fqtopic(ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); mangled_rp_topic_name = \ make_fqtopic(ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false); demangle_type = _demangle_if_ros_type; } - - rmw_topic_endpoint_info_array_t publishers_info = \ - rmw_get_zero_initialized_topic_endpoint_info_array(); - rmw_ret_t ret = common_context->graph_cache.get_writers_info_by_topic( - mangled_rq_topic_name, - demangle_type, - allocator, - &publishers_info); - std::unique_ptr< - rmw_topic_endpoint_info_array_t, - std::function> - publishers_info_delete_on_error( - &publishers_info, - [allocator](rmw_topic_endpoint_info_array_t * p) { - rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( - p, - allocator - ); - if (RMW_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy publishers_info when function failed."); - } - } - ); - if (RMW_RET_OK != ret) { - return ret; - } - - rmw_topic_endpoint_info_array_t subscriptions_info = \ - rmw_get_zero_initialized_topic_endpoint_info_array(); - ret = common_context->graph_cache.get_readers_info_by_topic( + return common_context->graph_cache.get_readers_info_by_topic( mangled_rp_topic_name, demangle_type, allocator, - &subscriptions_info); - std::unique_ptr< - rmw_topic_endpoint_info_array_t, - std::function> - subscriptions_info_delete_on_error( - &subscriptions_info, - [allocator](rmw_topic_endpoint_info_array_t * p) { - rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( - p, - allocator - ); - if (RMW_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy subscriptions_info when function failed."); - } - } - ); - if (RMW_RET_OK != ret) { - return ret; - } - - size_t total_size = publishers_info.size + subscriptions_info.size; - ret = rmw_topic_endpoint_info_array_init_with_size(clients_info, total_size, allocator); - std::unique_ptr< - rmw_topic_endpoint_info_array_t, - std::function> - clients_info_delete_on_error( - clients_info, - [allocator](rmw_topic_endpoint_info_array_t * p) { - rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( - p, - allocator - ); - if (RMW_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy clients_info when function failed."); - } - } - ); - if (RMW_RET_OK != ret) { - return ret; - } - for (size_t i = 0; i < publishers_info.size; ++i) { - clients_info->info_array[i] = publishers_info.info_array[i]; - } - for (size_t i = 0; i < subscriptions_info.size; ++i) { - clients_info->info_array[publishers_info.size + i] = subscriptions_info.info_array[i]; - } - publishers_info_delete_on_error.release(); - subscriptions_info_delete_on_error.release(); - clients_info_delete_on_error.release(); - return RMW_RET_OK; + clients_info); } extern "C" rmw_ret_t rmw_get_servers_info_by_service( @@ -5970,97 +5889,18 @@ extern "C" rmw_ret_t rmw_get_servers_info_by_service( auto common_context = &node->context->impl->common; - std::string mangled_rq_topic_name, mangled_rp_topic_name; - mangled_rq_topic_name = mangled_rp_topic_name = service_name; + std::string mangled_rp_topic_name = service_name; DemangleFunction demangle_type = _identity_demangle; if (!no_mangle) { - mangled_rq_topic_name = \ - make_fqtopic(ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); mangled_rp_topic_name = \ make_fqtopic(ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false); demangle_type = _demangle_if_ros_type; } - rmw_topic_endpoint_info_array_t subscriptions_info = \ - rmw_get_zero_initialized_topic_endpoint_info_array(); - rmw_ret_t ret = common_context->graph_cache.get_readers_info_by_topic( - mangled_rq_topic_name, - demangle_type, - allocator, - &subscriptions_info); - std::unique_ptr< - rmw_topic_endpoint_info_array_t, - std::function> - subscriptions_info_delete_on_error( - &subscriptions_info, - [allocator](rmw_topic_endpoint_info_array_t * p) { - rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( - p, - allocator - ); - if (RMW_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy subscriptions_info when function failed."); - } - } - ); - if (RMW_RET_OK != ret) { - return ret; - } - rmw_topic_endpoint_info_array_t publishers_info = \ - rmw_get_zero_initialized_topic_endpoint_info_array(); - ret = common_context->graph_cache.get_writers_info_by_topic( + return common_context->graph_cache.get_writers_info_by_topic( mangled_rp_topic_name, demangle_type, allocator, - &publishers_info); - std::unique_ptr< - rmw_topic_endpoint_info_array_t, - std::function> - publishers_info_delete_on_error( - &publishers_info, - [allocator](rmw_topic_endpoint_info_array_t * p) { - rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( - p, - allocator - ); - if (RMW_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy publishers_info when function failed."); - } - } - ); - if (RMW_RET_OK != ret) { - return ret; - } - - size_t total_size = publishers_info.size + subscriptions_info.size; - ret = rmw_topic_endpoint_info_array_init_with_size(servers_info, total_size, allocator); - std::unique_ptr< - rmw_topic_endpoint_info_array_t, - std::function> - servers_info_delete_on_error( - servers_info, - [allocator](rmw_topic_endpoint_info_array_t * p) { - rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( - p, - allocator - ); - if (RMW_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy servers_info when function failed."); - } - } - ); - if (RMW_RET_OK != ret) { - return ret; - } - for (size_t i = 0; i < publishers_info.size; ++i) { - servers_info->info_array[i] = publishers_info.info_array[i]; - } - for (size_t i = 0; i < subscriptions_info.size; ++i) { - servers_info->info_array[publishers_info.size + i] = subscriptions_info.info_array[i]; - } - publishers_info_delete_on_error.release(); - subscriptions_info_delete_on_error.release(); - servers_info_delete_on_error.release(); - return RMW_RET_OK; + servers_info); } extern "C" rmw_ret_t rmw_qos_profile_check_compatible(