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

@@ -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();