From 57bcea1e90ca03f2361cec03cc0d389d9b166a8b Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Tue, 25 Jun 2024 21:25:34 +0900 Subject: [PATCH] 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 *****************************************************************************/