From 7d7df288c86ed70d0b8657361b0ea3195d4ec950 Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Thu, 18 Jan 2024 13:54:58 +0800
Subject: [PATCH] Implement rmw_subscription_count_matched_publishers

Signed-off-by: Yadunund <yadunund@openrobotics.org>
---
 rmw_zenoh_cpp/src/detail/graph_cache.cpp | 20 ++++++++++++++++++++
 rmw_zenoh_cpp/src/detail/graph_cache.hpp |  4 ++++
 rmw_zenoh_cpp/src/rmw_zenoh.cpp          | 19 +++++++++++++++----
 3 files changed, 39 insertions(+), 4 deletions(-)

diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
index 1cb4288a..51c75a6e 100644
--- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp
+++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
@@ -732,6 +732,26 @@ rmw_ret_t GraphCache::publisher_count_matched_subscriptions(
   return RMW_RET_OK;
 }
 
+///=============================================================================
+rmw_ret_t GraphCache::subscription_count_matched_publishers(
+  const rmw_subscription_t * subscription,
+  size_t * publisher_count)
+{
+  // TODO(Yadunund): Check if QoS settings also match.
+  *publisher_count = 0;
+  GraphNode::TopicMap::const_iterator topic_it = graph_topics_.find(subscription->topic_name);
+  if (topic_it != graph_topics_.end()) {
+    rmw_subscription_data_t * sub_data = static_cast<rmw_subscription_data_t *>(subscription->data);
+    GraphNode::TopicDataMap::const_iterator topic_data_it = topic_it->second.find(
+      sub_data->type_support->get_name());
+    if (topic_data_it != topic_it->second.end()) {
+      *publisher_count = topic_data_it->second->stats_.pub_count_;
+    }
+  }
+
+  return RMW_RET_OK;
+}
+
 ///=============================================================================
 rmw_ret_t GraphCache::get_service_names_and_types(
   rcutils_allocator_t * allocator,
diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp
index 7c19700d..b9c5a2ac 100644
--- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp
+++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp
@@ -110,6 +110,10 @@ class GraphCache final
     const rmw_publisher_t * publisher,
     size_t * subscription_count);
 
+  rmw_ret_t subscription_count_matched_publishers(
+    const rmw_subscription_t * subscription,
+    size_t * publisher_count);
+
   rmw_ret_t get_service_names_and_types(
     rcutils_allocator_t * allocator,
     rmw_names_and_types_t * service_names_and_types) const;
diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp
index 56edc921..0ebaf07f 100644
--- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp
+++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp
@@ -1518,12 +1518,23 @@ rmw_subscription_count_matched_publishers(
   const rmw_subscription_t * subscription,
   size_t * publisher_count)
 {
-  static_cast<void>(subscription);
+  RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(subscription->data, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    subscription,
+    subscription->implementation_identifier,
+    rmw_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(publisher_count, RMW_RET_INVALID_ARGUMENT);
 
-  // TODO(clalancette): implement
-  *publisher_count = 0;
+  rmw_subscription_data_t * sub_data = static_cast<rmw_subscription_data_t *>(subscription->data);
+  RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(sub_data->context, RMW_RET_INVALID_ARGUMENT);
+  rmw_context_impl_t * context_impl = static_cast<rmw_context_impl_t *>(sub_data->context->impl);
+  RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT);
 
-  return RMW_RET_UNSUPPORTED;
+  return context_impl->graph_cache.subscription_count_matched_publishers(
+    subscription, publisher_count);
 }
 
 //==============================================================================