From 12f4add38c25c970821f3932cb9b8e86e9720d83 Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Tue, 25 Jun 2024 21:25:34 +0900 Subject: [PATCH 1/2] add : get clients,servers info Signed-off-by: Minju, Lee --- rmw_connextdds/src/rmw_api_impl_ndds.cpp | 26 ++ .../include/rmw_connextdds/rmw_api_impl.hpp | 18 ++ rmw_connextdds_common/src/common/rmw_info.cpp | 245 ++++++++++++++++++ .../src/rmw_api_impl_rtime.cpp | 25 ++ 4 files changed, 314 insertions(+) diff --git a/rmw_connextdds/src/rmw_api_impl_ndds.cpp b/rmw_connextdds/src/rmw_api_impl_ndds.cpp index e44df000..479867bd 100644 --- a/rmw_connextdds/src/rmw_api_impl_ndds.cpp +++ b/rmw_connextdds/src/rmw_api_impl_ndds.cpp @@ -316,6 +316,32 @@ rmw_get_subscriptions_info_by_topic( node, allocator, topic_name, no_mangle, subscriptions_info); } + +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) +{ + return rmw_api_connextdds_get_clients_info_by_service( + node, allocator, service_name, no_mangle, clients_info); +} + + +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) +{ + return rmw_api_connextdds_get_servers_info_by_service( + node, allocator, service_name, no_mangle, servers_info); +} + /***************************************************************************** * Node API *****************************************************************************/ diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp index 804b5653..a1822e09 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp @@ -220,6 +220,24 @@ rmw_api_connextdds_get_subscriptions_info_by_topic( bool no_mangle, rmw_topic_endpoint_info_array_t * subscriptions_info); +RMW_CONNEXTDDS_PUBLIC +rmw_ret_t +rmw_api_connextdds_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_CONNEXTDDS_PUBLIC +rmw_ret_t +rmw_api_connextdds_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); + /***************************************************************************** * Node API *****************************************************************************/ diff --git a/rmw_connextdds_common/src/common/rmw_info.cpp b/rmw_connextdds_common/src/common/rmw_info.cpp index 0418acf9..89631533 100644 --- a/rmw_connextdds_common/src/common/rmw_info.cpp +++ b/rmw_connextdds_common/src/common/rmw_info.cpp @@ -670,3 +670,248 @@ rmw_api_connextdds_get_subscriptions_info_by_topic( allocator, subscriptions_info); } + + +rmw_ret_t +rmw_api_connextdds_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, + RMW_CONNEXTDDS_ID, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(clients_info, RMW_RET_INVALID_ARGUMENT); + + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return 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 = + rmw_connextdds_create_topic_name( + ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); + mangled_rp_topic_name = + rmw_connextdds_create_topic_name( + 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; +} + + +rmw_ret_t +rmw_api_connextdds_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, + RMW_CONNEXTDDS_ID, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(servers_info, RMW_RET_INVALID_ARGUMENT); + + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return 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 = + rmw_connextdds_create_topic_name( + ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); + mangled_rp_topic_name = + rmw_connextdds_create_topic_name( + 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; +} diff --git a/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp b/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp index 3aaf5e95..4448fdc6 100644 --- a/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp +++ b/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp @@ -318,6 +318,31 @@ rmw_get_subscriptions_info_by_topic( node, allocator, topic_name, no_mangle, subscriptions_info); } + +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) +{ + return rmw_api_connextdds_get_clients_info_by_service( + node, allocator, service_name, no_mangle, clients_info); +} + + +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) +{ + return rmw_api_connextdds_get_servers_info_by_service( + node, allocator, service_name, no_mangle, servers_info); +} /***************************************************************************** * Node API *****************************************************************************/ From 9a8fb3785da18a407496358cf0b0b64b60c9e8e8 Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Fri, 27 Sep 2024 22:38:40 +0900 Subject: [PATCH 2/2] Fixes service info Signed-off-by: Minju, Lee --- rmw_connextdds_common/src/common/rmw_info.cpp | 173 +----------------- 1 file changed, 6 insertions(+), 167 deletions(-) diff --git a/rmw_connextdds_common/src/common/rmw_info.cpp b/rmw_connextdds_common/src/common/rmw_info.cpp index 89631533..b395b56d 100644 --- a/rmw_connextdds_common/src/common/rmw_info.cpp +++ b/rmw_connextdds_common/src/common/rmw_info.cpp @@ -698,99 +698,19 @@ rmw_api_connextdds_get_clients_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 = - rmw_connextdds_create_topic_name( - ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); mangled_rp_topic_name = rmw_connextdds_create_topic_name( 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); } @@ -820,98 +740,17 @@ rmw_api_connextdds_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 = - rmw_connextdds_create_topic_name( - ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); mangled_rp_topic_name = rmw_connextdds_create_topic_name( 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); }