diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 841fc055..751e66ac 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(rmw_zenoh_cpp SHARED src/rmw_event.cpp src/rmw_get_network_flow_endpoints.cpp src/rmw_get_node_info_and_types.cpp + src/rmw_get_service_endpoint_info.cpp src/rmw_get_service_names_and_types.cpp src/rmw_get_topic_endpoint_info.cpp src/rmw_get_topic_names_and_types.cpp diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index e406d2d6..58d0abce 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -1148,6 +1148,116 @@ rmw_ret_t GraphCache::get_entities_info_by_topic( return RMW_RET_OK; } +///============================================================================= +rmw_ret_t GraphCache::get_entities_info_by_service( + liveliness::EntityType entity_type, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_demangle, + rmw_service_endpoint_info_array_t * endpoints_info) const +{ + static_cast(no_demangle); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + + if (entity_type != EntityType::Client && entity_type != EntityType::Service) { + return RMW_RET_INVALID_ARGUMENT; + } + std::lock_guard lock(graph_mutex_); + + GraphNode::TopicMap::const_iterator service_it = graph_services_.find(service_name); + // Exit early if the topic does not exist in the graph. + if (service_it == graph_services_.end()) { + return RMW_RET_OK; + } + + std::vector endpoints; + for (const auto & [topic_type_name, topic_qos_map] : service_it->second) { + for (const auto & [_, topic_data] : topic_qos_map) { + const TopicData::EntitySet & entity_set = + entity_type == EntityType::Client ? topic_data->pubs_ : + topic_data->subs_; + for (const liveliness::ConstEntityPtr & entity : entity_set) { + rmw_service_endpoint_info_t ep = rmw_get_zero_initialized_service_endpoint_info(); + + rmw_ret_t ret = rmw_service_endpoint_info_set_node_name( + &ep, entity->node_name().c_str(), allocator); + if (RMW_RET_OK != ret) { + return ret; + } + + ret = rmw_service_endpoint_info_set_node_namespace( + &ep, entity->node_namespace().c_str(), allocator); + if (RMW_RET_OK != ret) { + return ret; + } + + ret = rmw_service_endpoint_info_set_service_type( + &ep, _demangle_if_ros_type(topic_type_name).c_str(), allocator); + if (RMW_RET_OK != ret) { + return ret; + } + + ret = rmw_service_endpoint_info_set_endpoint_type( + &ep, + entity_type == + EntityType::Client ? RMW_ENDPOINT_CLIENT : RMW_ENDPOINT_SERVER); + if (RMW_RET_OK != ret) { + return ret; + } + + ret = rmw_service_endpoint_info_set_endpoint_count(&ep, 1); + if (RMW_RET_OK != ret) { + return ret; + } + + ret = rmw_service_endpoint_info_set_qos_profiles( + &ep, &topic_data->info_.qos_, 1, allocator); + if (RMW_RET_OK != ret) { + return ret; + } + + rosidl_type_hash_t type_hash; + rcutils_ret_t rc_ret = rosidl_parse_type_hash_string( + topic_data->info_.type_hash_.c_str(), + &type_hash); + + if (RCUTILS_RET_OK == rc_ret) { + ret = rmw_service_endpoint_info_set_service_type_hash(&ep, &type_hash); + if (RMW_RET_OK != ret) { + return ret; + } + } + + ret = rmw_service_endpoint_info_set_gids( + &ep, + entity->copy_gid().data(), + 1, + RMW_GID_STORAGE_SIZE, + allocator); + if (RMW_RET_OK != ret) { + return ret; + } + + endpoints.push_back(ep); + } + } + } + + rmw_ret_t ret = rmw_service_endpoint_info_array_init_with_size( + endpoints_info, endpoints.size(), allocator); + if (RMW_RET_OK != ret) { + return ret; + } + + memcpy( + endpoints_info->info_array, &endpoints[0], + sizeof(rmw_service_endpoint_info_t) * endpoints.size()); + + return RMW_RET_OK; +} + ///============================================================================= rmw_ret_t GraphCache::service_server_is_available( const liveliness::TopicInfo & client_topic_info, diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index dc462b0d..c1e2d121 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -32,6 +32,7 @@ #include "rcutils/types.h" #include "rmw/rmw.h" +#include "rmw/get_service_endpoint_info.h" #include "rmw/get_topic_endpoint_info.h" #include "rmw/names_and_types.h" @@ -169,6 +170,13 @@ class GraphCache final bool no_demangle, rmw_topic_endpoint_info_array_t * endpoints_info) const; + rmw_ret_t get_entities_info_by_service( + liveliness::EntityType entity_type, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_demangle, + rmw_service_endpoint_info_array_t * endpoints_info) const; + rmw_ret_t service_server_is_available( const liveliness::TopicInfo & client_topic_info, bool * is_available) const; diff --git a/rmw_zenoh_cpp/src/rmw_get_service_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_service_endpoint_info.cpp new file mode 100644 index 00000000..e7b57399 --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_get_service_endpoint_info.cpp @@ -0,0 +1,88 @@ +// Copyright 2025 Minju Lee (이민주). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "detail/identifier.hpp" +#include "detail/liveliness_utils.hpp" +#include "detail/rmw_context_impl_s.hpp" + +#include "rcutils/allocator.h" + +#include "rmw/get_service_endpoint_info.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/types.h" + +extern "C" +{ +///============================================================================= +/// Retrieve endpoint information for each known client of a given service. +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_service_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_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + if (RMW_RET_OK != rmw_service_endpoint_info_array_check_zero(clients_info)) { + return RMW_RET_INVALID_ARGUMENT; + } + return context_impl->graph_cache()->get_entities_info_by_service( + rmw_zenoh_cpp::liveliness::EntityType::Client, + allocator, + service_name, + no_mangle, + clients_info); +} + +///============================================================================= +/// Retrieve endpoint information for each known server of a given service. +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_service_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_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + if (RMW_RET_OK != rmw_service_endpoint_info_array_check_zero(servers_info)) { + return RMW_RET_INVALID_ARGUMENT; + } + return context_impl->graph_cache()->get_entities_info_by_service( + rmw_zenoh_cpp::liveliness::EntityType::Service, + allocator, + service_name, + no_mangle, + servers_info); +} +} // extern "C"