From 4c3d4b1a518bc9b33c54840cb44f5398ce770bdb Mon Sep 17 00:00:00 2001
From: Guillaume Autran <gautran@clearpath.ai>
Date: Thu, 19 Jul 2018 13:39:30 -0400
Subject: [PATCH] Improve runtime performance of `rmw_count_XXX` functions
 (#216)

Change the runtime performance of `rmw_count_subscribers` and `rmw_count_publishers` to be O(logN) instead of linear.
---
 rmw_fastrtps_cpp/src/rmw_count.cpp | 40 +++++++++++++++++++++++-------
 1 file changed, 31 insertions(+), 9 deletions(-)

diff --git a/rmw_fastrtps_cpp/src/rmw_count.cpp b/rmw_fastrtps_cpp/src/rmw_count.cpp
index 0ed66c4e6..3ad587432 100644
--- a/rmw_fastrtps_cpp/src/rmw_count.cpp
+++ b/rmw_fastrtps_cpp/src/rmw_count.cpp
@@ -24,6 +24,7 @@
 
 #include "rmw_fastrtps_cpp/identifier.hpp"
 #include "demangle.hpp"
+#include "namespace_prefix.hpp"
 #include "rmw_fastrtps_cpp/custom_participant_info.hpp"
 
 extern "C"
@@ -49,12 +50,23 @@ rmw_count_publishers(
   auto impl = static_cast<CustomParticipantInfo *>(node->data);
 
   WriterInfo * slave_target = impl->secondaryPubListener;
-  slave_target->mapmutex.lock();
   *count = 0;
-  for (const auto & it : slave_target->topicNtypes) {
-    const auto topic_fqdn = _demangle_if_ros_topic(it.first);
-    if (topic_fqdn == topic_name) {
-      *count += it.second.size();
+  slave_target->mapmutex.lock();
+
+  // Build the list of all possible topic FQDN
+  std::vector<std::string> topic_fqdns;
+  topic_fqdns.push_back(topic_name);
+  if (topic_name[0] == '/') {
+    std::for_each(_ros_prefixes.begin(), _ros_prefixes.end(),
+      [&topic_fqdns, &topic_name](const std::string & prefix) {
+        topic_fqdns.push_back(prefix + topic_name);
+      });
+  }
+  // Search and sum up the publisher counts
+  for (const auto & topic_fqdn : topic_fqdns) {
+    const auto & it = slave_target->topicNtypes.find(topic_fqdn);
+    if (it != slave_target->topicNtypes.end()) {
+      *count += it->second.size();
     }
   }
   slave_target->mapmutex.unlock();
@@ -90,10 +102,20 @@ rmw_count_subscribers(
   ReaderInfo * slave_target = impl->secondarySubListener;
   *count = 0;
   slave_target->mapmutex.lock();
-  for (const auto & it : slave_target->topicNtypes) {
-    const auto topic_fqdn = _demangle_if_ros_topic(it.first);
-    if (topic_fqdn == topic_name) {
-      *count += it.second.size();
+  // Build the list of all possible topic FQDN
+  std::vector<std::string> topic_fqdns;
+  topic_fqdns.push_back(topic_name);
+  if (topic_name[0] == '/') {
+    std::for_each(_ros_prefixes.begin(), _ros_prefixes.end(),
+      [&topic_fqdns, &topic_name](const std::string & prefix) {
+        topic_fqdns.push_back(prefix + topic_name);
+      });
+  }
+  // Search and sum up the subscriber counts
+  for (const auto & topic_fqdn : topic_fqdns) {
+    const auto & it = slave_target->topicNtypes.find(topic_fqdn);
+    if (it != slave_target->topicNtypes.end()) {
+      *count += it->second.size();
     }
   }
   slave_target->mapmutex.unlock();