fix bug in deleting and counting topics

Signed-off-by: Yadunund <[email protected]>
ros2 · Nov 27, 2023 · c4fa36c · c4fa36c
1 parent 64a023d
commit c4fa36c
Show file tree

Hide file tree

Showing 3 changed files with 99 additions and 11 deletions.
diff --git 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<void>(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<void>(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<void>(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
@@ -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
@@ -2153,10 +2153,27 @@ rmw_count_publishers(
   const char * topic_name,
   size_t * count)
 {
-  static_cast<void>(node);
-  static_cast<void>(topic_name);
-  static_cast<void>(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<void>(node);
-  static_cast<void>(topic_name);
-  static_cast<void>(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);
 }
 
 //==============================================================================