Implement rmw_subscription_count_matched_publishers

Signed-off-by: Yadunund <[email protected]>
ros2 · Jan 18, 2024 · 7d7df28 · 7d7df28
1 parent 2099b8b
commit 7d7df28
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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);
 }
 
 //==============================================================================