Add rmw_get_clients_info_by_service and rmw_get_servers_info_by_service

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

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<void>(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<std::mutex> 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<rmw_service_endpoint_info_t> 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,

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;

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<rmw_context_impl_t *>(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<rmw_context_impl_t *>(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"