From 73d5e0fec84fa972eb0bf12ddafb7f2b33027b7a Mon Sep 17 00:00:00 2001
From: Yadunund <yadunund@openrobotics.org>
Date: Fri, 12 Jan 2024 01:27:06 +0800
Subject: [PATCH] Fix rmw_publisher_count_matched_subscriptions

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

diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
index 43166ff1..b22a1544 100644
--- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp
+++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
@@ -34,6 +34,7 @@
 #include "rmw/validate_node_name.h"
 
 #include "graph_cache.hpp"
+#include "rmw_data_types.hpp"
 
 ///=============================================================================
 using Entity = liveliness::Entity;
@@ -705,6 +706,26 @@ rmw_ret_t GraphCache::get_topic_names_and_types(
   return fill_names_and_types(graph_topics_, allocator, topic_names_and_types);
 }
 
+///=============================================================================
+rmw_ret_t GraphCache::publisher_count_matched_subscriptions(
+  const rmw_publisher_t * publisher,
+  size_t * subscription_count)
+{
+  // TODO(Yadunund): Check if QoS settings also match.
+  *subscription_count = 0;
+  GraphNode::TopicMap::const_iterator topic_it = graph_topics_.find(publisher->topic_name);
+  if (topic_it != graph_topics_.end()) {
+    rmw_publisher_data_t * pub_data = static_cast<rmw_publisher_data_t *>(publisher->data);
+    GraphNode::TopicDataMap::const_iterator topic_data_it = topic_it->second.find(
+      pub_data->type_support->get_name());
+    if (topic_data_it != topic_it->second.end()) {
+      *subscription_count = topic_data_it->second->stats_.sub_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 adeb3e01..7c19700d 100644
--- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp
+++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp
@@ -106,6 +106,10 @@ class GraphCache final
     bool no_demangle,
     rmw_names_and_types_t * topic_names_and_types) const;
 
+  rmw_ret_t publisher_count_matched_subscriptions(
+    const rmw_publisher_t * publisher,
+    size_t * subscription_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 377edbf7..87c5840c 100644
--- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp
+++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp
@@ -572,8 +572,6 @@ rmw_create_publisher(
       allocator->deallocate(const_cast<char *>(rmw_publisher->topic_name), allocator->state);
     });
 
-  // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate
-  // a z_publisher_put_options struct instead of passing NULL to this function.
   z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key(
     topic_name, node->context->actual_domain_id, allocator);
   auto always_free_ros_keyexpr = rcpputils::make_scope_exit(
@@ -954,11 +952,23 @@ rmw_publisher_count_matched_subscriptions(
   const rmw_publisher_t * publisher,
   size_t * subscription_count)
 {
-  static_cast<void>(publisher);
-  static_cast<void>(subscription_count);
-  // TODO(yadunund): Fixme.
-  *subscription_count = 0;
-  return RMW_RET_OK;
+  RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(publisher->data, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    publisher,
+    publisher->implementation_identifier,
+    rmw_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(subscription_count, RMW_RET_INVALID_ARGUMENT);
+
+  rmw_publisher_data_t * pub_data = static_cast<rmw_publisher_data_t *>(publisher->data);
+  RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(pub_data->context, RMW_RET_INVALID_ARGUMENT);
+  rmw_context_impl_t * context_impl = static_cast<rmw_context_impl_t *>(pub_data->context->impl);
+  RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT);
+
+  return context_impl->graph_cache.publisher_count_matched_subscriptions(
+    publisher, subscription_count);
 }
 
 //==============================================================================