Include node namespaces in get_node_names (#224)

* Include node namespaces in get_node_names. * Remove debug printf. * Address reviewer feedback. * Better error handling on get_node_names. * Check snprintf output. * Fix compiler warning.
ros2 · Sep 6, 2018 · 95a8c4d · 95a8c4d
1 parent 7bb2891
commit 95a8c4d
Show file tree

Hide file tree

Showing 6 changed files with 93 additions and 30 deletions.
diff --git a/rmw_fastrtps_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_cpp/src/rmw_node_names.cpp
@@ -33,9 +33,10 @@ extern "C"
 rmw_ret_t
 rmw_get_node_names(
   const rmw_node_t * node,
-  rcutils_string_array_t * node_names)
+  rcutils_string_array_t * node_names,
+  rcutils_string_array_t * node_namespaces)
 {
   return rmw_fastrtps_shared_cpp::__rmw_get_node_names(
-    eprosima_fastrtps_identifier, node, node_names);
+    eprosima_fastrtps_identifier, node, node_names, node_namespaces);
 }
 }  // extern "C"
diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp
@@ -33,9 +33,10 @@ extern "C"
 rmw_ret_t
 rmw_get_node_names(
   const rmw_node_t * node,
-  rcutils_string_array_t * node_names)
+  rcutils_string_array_t * node_names,
+  rcutils_string_array_t * node_namespaces)
 {
   return rmw_fastrtps_shared_cpp::__rmw_get_node_names(
-    eprosima_fastrtps_identifier, node, node_names);
+    eprosima_fastrtps_identifier, node, node_names, node_namespaces);
 }
 }  // extern "C"
diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp
@@ -58,25 +58,43 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener
       // ignore already known GUIDs
       if (discovered_names.find(info.rtps.m_guid) == discovered_names.end()) {
         auto map = rmw::impl::cpp::parse_key_value(info.rtps.m_userData);
-        auto found = map.find("name");
+        auto name_found = map.find("name");
+        auto ns_found = map.find("namespace");
+
         std::string name;
-        if (found != map.end()) {
-          name = std::string(found->second.begin(), found->second.end());
+        if (name_found != map.end()) {
+          name = std::string(name_found->second.begin(), name_found->second.end());
+        }
+
+        std::string namespace_;
+        if (ns_found != map.end()) {
+          namespace_ = std::string(ns_found->second.begin(), ns_found->second.end());
         }
+
         if (name.empty()) {
           // use participant name if no name was found in the user data
           name = info.rtps.m_RTPSParticipantName;
         }
         // ignore discovered participants without a name
         if (!name.empty()) {
           discovered_names[info.rtps.m_guid] = name;
+          discovered_namespaces[info.rtps.m_guid] = namespace_;
         }
       }
     } else {
-      auto it = discovered_names.find(info.rtps.m_guid);
-      // only consider known GUIDs
-      if (it != discovered_names.end()) {
-        discovered_names.erase(it);
+      {
+        auto it = discovered_names.find(info.rtps.m_guid);
+        // only consider known GUIDs
+        if (it != discovered_names.end()) {
+          discovered_names.erase(it);
+        }
+      }
+      {
+        auto it = discovered_namespaces.find(info.rtps.m_guid);
+        // only consider known GUIDs
+        if (it != discovered_namespaces.end()) {
+          discovered_namespaces.erase(it);
+        }
       }
     }
   }
@@ -91,7 +109,19 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener
     return names;
   }
 
+  std::vector<std::string> get_discovered_namespaces() const
+  {
+    std::vector<std::string> namespaces(discovered_namespaces.size());
+    size_t i = 0;
+    for (auto it : discovered_namespaces) {
+      namespaces[i++] = it.second;
+    }
+    return namespaces;
+  }
+
+
   std::map<eprosima::fastrtps::rtps::GUID_t, std::string> discovered_names;
+  std::map<eprosima::fastrtps::rtps::GUID_t, std::string> discovered_namespaces;
 };
 
 #endif  // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_
diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp
@@ -105,7 +105,8 @@ rmw_ret_t
 __rmw_get_node_names(
   const char * identifier,
   const rmw_node_t * node,
-  rcutils_string_array_t * node_names);
+  rcutils_string_array_t * node_names,
+  rcutils_string_array_t * node_namespaces);
 
 RMW_FASTRTPS_SHARED_CPP_PUBLIC
 rmw_ret_t

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp
@@ -251,15 +251,16 @@ __rmw_create_node(
     eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
   participantAttrs.rtps.builtin.writerHistoryMemoryPolicy =
     eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
-  // the node name is also set in the user_data
-  size_t name_length = strlen(name);
-  const char prefix[6] = "name=";
-  participantAttrs.rtps.userData.resize(name_length + sizeof(prefix));
-  memcpy(participantAttrs.rtps.userData.data(), prefix, sizeof(prefix) - 1);
-  for (size_t i = 0; i < name_length; ++i) {
-    participantAttrs.rtps.userData[sizeof(prefix) - 1 + i] = name[i];
+
+  size_t length = strlen(name) + strlen("name=;") +
+    strlen(namespace_) + strlen("namespace=;") + 1;
+  participantAttrs.rtps.userData.resize(length);
+  int written = snprintf(reinterpret_cast<char *>(participantAttrs.rtps.userData.data()),
+      length, "name=%s;namespace=%s;", name, namespace_);
+  if (written < 0 || written > static_cast<int>(length) - 1) {
+    RMW_SET_ERROR_MSG("failed to populate user_data buffer");
+    return nullptr;
   }
-  participantAttrs.rtps.userData[sizeof(prefix) - 1 + name_length] = ';';
 
   if (security_options->security_root_path) {
     // if security_root_path provided, try to find the key and certificate files

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp
@@ -38,7 +38,8 @@ rmw_ret_t
 __rmw_get_node_names(
   const char * identifier,
   const rmw_node_t * node,
-  rcutils_string_array_t * node_names)
+  rcutils_string_array_t * node_names,
+  rcutils_string_array_t * node_namespaces)
 {
   if (!node) {
     RMW_SET_ERROR_MSG("null node handle");
@@ -47,6 +48,9 @@ __rmw_get_node_names(
   if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) {
     return RMW_RET_ERROR;
   }
+  if (rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) {
+    return RMW_RET_ERROR;
+  }
 
   // Get participant pointer from node
   if (node->implementation_identifier != identifier) {
@@ -56,31 +60,56 @@ __rmw_get_node_names(
 
   auto impl = static_cast<CustomParticipantInfo *>(node->data);
   auto participant_names = impl->listener->get_discovered_names();
+  auto participant_ns = impl->listener->get_discovered_namespaces();
 
   rcutils_allocator_t allocator = rcutils_get_default_allocator();
   rcutils_ret_t rcutils_ret =
     rcutils_string_array_init(node_names, participant_names.size() + 1, &allocator);
   if (rcutils_ret != RCUTILS_RET_OK) {
     RMW_SET_ERROR_MSG(rcutils_get_error_string_safe())
-    return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret);
+    goto fail;
+  }
+
+  rcutils_ret =
+    rcutils_string_array_init(node_namespaces, participant_names.size() + 1, &allocator);
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    RMW_SET_ERROR_MSG(rcutils_get_error_string_safe())
+    goto fail;
   }
+
   for (size_t i = 0; i < participant_names.size() + 1; ++i) {
     if (0 == i) {
       node_names->data[i] = rcutils_strdup(node->name, allocator);
+      node_namespaces->data[i] = rcutils_strdup(node->namespace_, allocator);
     } else {
       node_names->data[i] = rcutils_strdup(participant_names[i - 1].c_str(), allocator);
+      node_namespaces->data[i] = rcutils_strdup(participant_ns[i - 1].c_str(), allocator);
     }
-    if (!node_names->data[i]) {
+    if (!node_names->data[i] || !node_namespaces->data[i]) {
       RMW_SET_ERROR_MSG("failed to allocate memory for node name")
-      rcutils_ret = rcutils_string_array_fini(node_names);
-      if (rcutils_ret != RCUTILS_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rmw_fastrtps_shared_cpp",
-          "failed to cleanup during error handling: %s", rcutils_get_error_string_safe())
-      }
-      return RMW_RET_BAD_ALLOC;
+      goto fail;
     }
   }
   return RMW_RET_OK;
+fail:
+  if (node_names) {
+    rcutils_ret = rcutils_string_array_fini(node_names);
+    if (rcutils_ret != RCUTILS_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rmw_connext_cpp",
+        "failed to cleanup during error handling: %s", rcutils_get_error_string_safe())
+      rcutils_reset_error();
+    }
+  }
+  if (node_namespaces) {
+    rcutils_ret = rcutils_string_array_fini(node_namespaces);
+    if (rcutils_ret != RCUTILS_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rmw_connext_cpp",
+        "failed to cleanup during error handling: %s", rcutils_get_error_string_safe())
+      rcutils_reset_error();
+    }
+  }
+  return RMW_RET_BAD_ALLOC;
 }
 }  // namespace rmw_fastrtps_shared_cpp