From c4fa36c4e5350b46b28ff272217270bf757ac3d2 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 27 Nov 2023 23:47:33 +0800 Subject: [PATCH] fix bug in deleting and counting topics Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 48 ++++++++++++++++++++++- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 12 +++++- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 50 ++++++++++++++++++++---- 3 files changed, 99 insertions(+), 11 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index c87d664a..1ab1c780 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -282,7 +282,11 @@ void GraphCache::parse_del(const std::string & keyexpr) if (cache_topic_data_it->second->stats_.pub_count_ == 0 && cache_topic_data_it->second->stats_.sub_count_ == 0) { - graph_topics_.erase(entity.topic_info()->name_); + cache_topic_it->second.erase(cache_topic_data_it); + } + // If the topic does not have any TopicData entries, erase the topic from the map. + if (cache_topic_it->second.empty()) { + graph_topics_.erase(cache_topic_it); } } @@ -495,7 +499,7 @@ _demangle_if_ros_type(const std::string & dds_type_string) rmw_ret_t GraphCache::get_topic_names_and_types( rcutils_allocator_t * allocator, bool no_demangle, - rmw_names_and_types_t * topic_names_and_types) + rmw_names_and_types_t * topic_names_and_types) const { static_cast(no_demangle); RCUTILS_CHECK_ALLOCATOR_WITH_MSG( @@ -550,3 +554,43 @@ rmw_ret_t GraphCache::get_topic_names_and_types( return RMW_RET_OK; } + +///============================================================================= +rmw_ret_t GraphCache::count_publishers( + const rmw_node_t * node, + const char * topic_name, + size_t * count) const +{ + static_cast(node); + *count = 0; + auto topic_it = graph_topics_.find(topic_name); + if (topic_it == graph_topics_.end()) { + return RMW_RET_OK; + } + for (const auto & it : topic_it->second) { + // Iterate through all the types and increment count. + *count += it.second->stats_.pub_count_; + } + + return RMW_RET_OK; +} + +///============================================================================= +rmw_ret_t GraphCache::count_subscriptions( + const rmw_node_t * node, + const char * topic_name, + size_t * count) const +{ + static_cast(node); + *count = 0; + auto topic_it = graph_topics_.find(topic_name); + if (topic_it == graph_topics_.end()) { + return RMW_RET_OK; + } + for (const auto & it : topic_it->second) { + // Iterate through all the types and increment count. + *count += it.second->stats_.sub_count_; + } + + return RMW_RET_OK; +} diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 7ed00705..d9010711 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -91,7 +91,17 @@ class GraphCache final rmw_ret_t get_topic_names_and_types( rcutils_allocator_t * allocator, bool no_demangle, - rmw_names_and_types_t * topic_names_and_types); + rmw_names_and_types_t * topic_names_and_types) const; + + rmw_ret_t count_publishers( + const rmw_node_t * node, + const char * topic_name, + size_t * count) const; + + rmw_ret_t count_subscriptions( + const rmw_node_t * node, + const char * topic_name, + size_t * count) const; private: /* diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 23973046..a18972e6 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -2153,10 +2153,27 @@ rmw_count_publishers( const char * topic_name, size_t * count) { - static_cast(node); - static_cast(topic_name); - static_cast(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(topic_name, RMW_RET_INVALID_ARGUMENT); + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_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_publishers( + node, topic_name, count); } //============================================================================== @@ -2167,10 +2184,27 @@ rmw_count_subscribers( const char * topic_name, size_t * count) { - static_cast(node); - static_cast(topic_name); - static_cast(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(topic_name, RMW_RET_INVALID_ARGUMENT); + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_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_subscriptions( + node, topic_name, count); } //==============================================================================