add : get clients, servers info

Signed-off-by: Minju, Lee <[email protected]>
ros2 · Jun 25, 2024 · 42f28b6 · 42f28b6
1 parent 1417a44
commit 42f28b6
Show file tree

Hide file tree

Showing 4 changed files with 292 additions and 0 deletions.
diff --git a/rmw_fastrtps_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_cpp/src/rmw_get_topic_endpoint_info.cpp
@@ -43,4 +43,28 @@ rmw_get_subscriptions_info_by_topic(
   return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic(
     eprosima_fastrtps_identifier, 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_fastrtps_shared_cpp::__rmw_get_clients_info_by_service(
+    eprosima_fastrtps_identifier, 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_fastrtps_shared_cpp::__rmw_get_servers_info_by_service(
+    eprosima_fastrtps_identifier, node, allocator, service_name, no_mangle, servers_info);
+}
 }  // extern "C"
diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_endpoint_info.cpp
@@ -43,4 +43,28 @@ rmw_get_subscriptions_info_by_topic(
   return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic(
     eprosima_fastrtps_identifier, 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_fastrtps_shared_cpp::__rmw_get_clients_info_by_service(
+    eprosima_fastrtps_identifier, 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_fastrtps_shared_cpp::__rmw_get_servers_info_by_service(
+    eprosima_fastrtps_identifier, node, allocator, service_name, no_mangle, servers_info);
+}
 }  // 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
@@ -523,6 +523,26 @@ __rmw_get_subscriptions_info_by_topic(
   bool no_mangle,
   rmw_topic_endpoint_info_array_t * subscriptions_info);
 
+RMW_FASTRTPS_SHARED_CPP_PUBLIC
+rmw_ret_t
+__rmw_get_clients_info_by_service(
+  const char * identifier,
+  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_FASTRTPS_SHARED_CPP_PUBLIC
+rmw_ret_t
+__rmw_get_servers_info_by_service(
+  const char * identifier,
+  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_FASTRTPS_SHARED_CPP_PUBLIC
 rmw_ret_t
 __rmw_qos_profile_check_compatible(

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp
@@ -123,4 +123,228 @@ __rmw_get_subscriptions_info_by_topic(
     allocator,
     subscriptions_info);
 }
+
+rmw_ret_t
+__rmw_get_clients_info_by_service(
+  const char * identifier,
+  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_ret_t ret = __validate_arguments(
+    identifier,
+    node,
+    allocator,
+    service_name,
+    clients_info);
+  if (ret != RMW_RET_OK) {
+    return ret;
+  }
+  auto common_context = static_cast<rmw_dds_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 = \
+      _mangle_topic_name(ros_service_requester_prefix, service_name, "Request").to_string();
+    mangled_rp_topic_name = \
+      _mangle_topic_name(ros_service_response_prefix, service_name, "Reply").to_string();
+    demangle_type = _demangle_if_ros_type;
+  }
+  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_rq_topic_name,
+    demangle_type,
+    allocator,
+    &publishers_info);
+  std::unique_ptr<
+    rmw_topic_endpoint_info_array_t,
+    std::function<void(rmw_topic_endpoint_info_array_t *)>>
+  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<void(rmw_topic_endpoint_info_array_t *)>>
+  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<void(rmw_topic_endpoint_info_array_t *)>>
+  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_get_servers_info_by_service(
+  const char * identifier,
+  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_ret_t ret = __validate_arguments(
+    identifier,
+    node,
+    allocator,
+    service_name,
+    servers_info);
+  if (ret != RMW_RET_OK) {
+    return ret;
+  }
+  auto common_context = static_cast<rmw_dds_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 = \
+      _mangle_topic_name(ros_service_requester_prefix, service_name, "Request").to_string();
+    mangled_rp_topic_name = \
+      _mangle_topic_name(ros_service_response_prefix, service_name, "Reply").to_string();
+    demangle_type = _demangle_if_ros_type;
+  }
+  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_rq_topic_name,
+    demangle_type,
+    allocator,
+    &subscriptions_info);
+  std::unique_ptr<
+    rmw_topic_endpoint_info_array_t,
+    std::function<void(rmw_topic_endpoint_info_array_t *)>>
+  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<void(rmw_topic_endpoint_info_array_t *)>>
+  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<void(rmw_topic_endpoint_info_array_t *)>>
+  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;
+}
 }  // namespace rmw_fastrtps_shared_cpp