Implement count methods for services and clients

Signed-off-by: Yadunund <[email protected]>
ros2 · Jan 12, 2024 · 2175a27 · 2175a27
1 parent 9cca466
commit 2175a27
Show file tree

Hide file tree

Showing 3 changed files with 82 additions and 8 deletions.
diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
@@ -754,6 +754,40 @@ rmw_ret_t GraphCache::count_subscriptions(
   return RMW_RET_OK;
 }
 
+///=============================================================================
+rmw_ret_t GraphCache::count_services(
+  const char * service_name,
+  size_t * count) const
+{
+  *count = 0;
+  std::lock_guard<std::mutex> lock(graph_mutex_);
+  if (graph_services_.count(service_name) != 0) {
+    for (const std::pair<const std::string, TopicDataPtr> & it : graph_services_.at(service_name)) {
+      // Iterate through all the types and increment count.
+      *count += it.second->stats_.sub_count_;
+    }
+  }
+
+  return RMW_RET_OK;
+}
+
+///=============================================================================
+rmw_ret_t GraphCache::count_clients(
+  const char * service_name,
+  size_t * count) const
+{
+  *count = 0;
+  std::lock_guard<std::mutex> lock(graph_mutex_);
+  if (graph_services_.count(service_name) != 0) {
+    for (const std::pair<const std::string, TopicDataPtr> & it : graph_services_.at(service_name)) {
+      // Iterate through all the types and increment count.
+      *count += it.second->stats_.pub_count_;
+    }
+  }
+
+  return RMW_RET_OK;
+}
+
 ///=============================================================================
 rmw_ret_t GraphCache::get_entity_names_and_types_by_node(
   liveliness::EntityType entity_type,

diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp
@@ -118,6 +118,14 @@ class GraphCache final
     const char * topic_name,
     size_t * count) const;
 
+  rmw_ret_t count_services(
+    const char * service_name,
+    size_t * count) const;
+
+  rmw_ret_t count_clients(
+    const char * service_name,
+    size_t * count) const;
+
   rmw_ret_t get_entity_names_and_types_by_node(
     liveliness::EntityType entity_type,
     rcutils_allocator_t * allocator,

diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp
@@ -3208,10 +3208,26 @@ rmw_count_clients(
   const char * service_name,
   size_t * count)
 {
-  static_cast<void>(node);
-  static_cast<void>(service_name);
-  static_cast<void>(count);
-  return RMW_RET_UNSUPPORTED;
+  RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node,
+    node->implementation_identifier,
+    rmw_zenoh_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("topic_name argument is invalid: %s", reason);
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+  RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT);
+
+  return node->context->impl->graph_cache.count_clients(service_name, count);
 }
 
 //==============================================================================
@@ -3222,10 +3238,26 @@ rmw_count_services(
   const char * service_name,
   size_t * count)
 {
-  static_cast<void>(node);
-  static_cast<void>(service_name);
-  static_cast<void>(count);
-  return RMW_RET_UNSUPPORTED;
+  RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node,
+    node->implementation_identifier,
+    rmw_zenoh_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("topic_name argument is invalid: %s", reason);
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+  RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT);
+
+  return node->context->impl->graph_cache.count_services(service_name, count);
 }
 
 //==============================================================================