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(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(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(publisher); - static_cast(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(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(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); } //==============================================================================