Add rmw_count clients,services impl (#641)

* Add rmw_count clients,services impl Signed-off-by: leeminju531 <[email protected]>
ros2 · Sep 29, 2023 · ad4550a · ad4550a
1 parent b6c069c
commit ad4550a
Show file tree

Hide file tree

Showing 4 changed files with 118 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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<rmw_dds_common::Context *>(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<rmw_dds_common::Context *>(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