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_shared_cpp/src/rmw_count.cpp

@@ -23,6 +23,7 @@
 #include "rmw/types.h"
 
 #include "demangle.hpp"
+#include "namespace_prefix.hpp"
 #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
 #include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
 #include "reader_info.hpp"
@@ -54,12 +55,24 @@ __rmw_count_publishers(
   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();
+
+  // 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();
 
   RCUTILS_LOG_DEBUG_NAMED(
@@ -94,10 +107,21 @@ __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();