From ad4550adea4f65fdc126423f2fec13f441c6bbaa Mon Sep 17 00:00:00 2001 From: "Minju, Lee" <70446214+leeminju531@users.noreply.github.com> Date: Fri, 29 Sep 2023 21:35:29 +0900 Subject: [PATCH] Add rmw_count clients,services impl (#641) * Add rmw_count clients,services impl Signed-off-by: leeminju531 --- rmw_fastrtps_cpp/src/rmw_count.cpp | 20 ++++++ rmw_fastrtps_dynamic_cpp/src/rmw_count.cpp | 20 ++++++ .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 16 +++++ rmw_fastrtps_shared_cpp/src/rmw_count.cpp | 62 +++++++++++++++++++ 4 files changed, 118 insertions(+) diff --git a/rmw_fastrtps_cpp/src/rmw_count.cpp b/rmw_fastrtps_cpp/src/rmw_count.cpp index cf61bd1cc..cdfd4839c 100644 --- a/rmw_fastrtps_cpp/src/rmw_count.cpp +++ b/rmw_fastrtps_cpp/src/rmw_count.cpp @@ -47,4 +47,24 @@ rmw_count_subscribers( return rmw_fastrtps_shared_cpp::__rmw_count_subscribers( eprosima_fastrtps_identifier, node, topic_name, count); } + +rmw_ret_t +rmw_count_clients( + const rmw_node_t * node, + const char * service_name, + size_t * count) +{ + return rmw_fastrtps_shared_cpp::__rmw_count_clients( + eprosima_fastrtps_identifier, node, service_name, count); +} + +rmw_ret_t +rmw_count_services( + const rmw_node_t * node, + const char * service_name, + size_t * count) +{ + return rmw_fastrtps_shared_cpp::__rmw_count_services( + eprosima_fastrtps_identifier, node, service_name, count); +} } // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_count.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_count.cpp index c5dacef08..21c9cfd04 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_count.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_count.cpp @@ -47,4 +47,24 @@ rmw_count_subscribers( return rmw_fastrtps_shared_cpp::__rmw_count_subscribers( eprosima_fastrtps_identifier, node, topic_name, count); } + +rmw_ret_t +rmw_count_clients( + const rmw_node_t * node, + const char * service_name, + size_t * count) +{ + return rmw_fastrtps_shared_cpp::__rmw_count_clients( + eprosima_fastrtps_identifier, node, service_name, count); +} + +rmw_ret_t +rmw_count_services( + const rmw_node_t * node, + const char * service_name, + size_t * count) +{ + return rmw_fastrtps_shared_cpp::__rmw_count_services( + eprosima_fastrtps_identifier, node, service_name, count); +} } // extern "C" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index ff2ca03c9..2994c108f 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -60,6 +60,22 @@ __rmw_count_subscribers( const char * topic_name, size_t * count); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_count_clients( + const char * identifier, + const rmw_node_t * node, + const char * service_name, + size_t * count); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_count_services( + const char * identifier, + const rmw_node_t * node, + const char * service_name, + size_t * count); + RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_get_gid_for_publisher( diff --git a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp index 59b5bd962..b24f12e68 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp @@ -97,4 +97,66 @@ __rmw_count_subscribers( _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); return common_context->graph_cache.get_reader_count(mangled_topic_name, count); } + +rmw_ret_t +__rmw_count_clients( + const char * identifier, + const rmw_node_t * node, + const char * service_name, + size_t * count) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(service_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return ret; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("service_name argument is invalid: %s", reason); + return RMW_RET_INVALID_ARGUMENT; + } + RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); + auto common_context = static_cast(node->context->impl->common); + const std::string mangled_rp_service_name = + _mangle_topic_name(ros_service_response_prefix, service_name, "Reply").to_string(); + return common_context->graph_cache.get_reader_count(mangled_rp_service_name, count); +} + +rmw_ret_t +__rmw_count_services( + const char * identifier, + const rmw_node_t * node, + const char * service_name, + size_t * count) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(service_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return ret; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("service_name argument is invalid: %s", reason); + return RMW_RET_INVALID_ARGUMENT; + } + RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); + auto common_context = static_cast(node->context->impl->common); + const std::string mangled_rp_service_name = + _mangle_topic_name(ros_service_response_prefix, service_name, "Reply").to_string(); + return common_context->graph_cache.get_writer_count(mangled_rp_service_name, count); +} } // namespace rmw_fastrtps_shared_cpp