Include node namespaces in get_node_names (ros2#299)

* Include node namespaces in get_node_names. * Address reviewer feedback. * Use goto to handle failure conditions * Fix typo. Signed-off-by: Devin Bonnie <[email protected]>
aws-ros-dev · Apr 3, 2019 · 160f17d · 160f17d
1 parent 0d63bce
commit 160f17d
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 24 deletions.
diff --git a/rmw_connext_cpp/src/rmw_node_names.cpp b/rmw_connext_cpp/src/rmw_node_names.cpp
@@ -25,8 +25,9 @@ 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 get_node_names(rti_connext_identifier, node, node_names);
+  return get_node_names(rti_connext_identifier, node, node_names, node_namespaces);
 }
 }  // extern "C"
diff --git a/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/node_names.hpp b/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/node_names.hpp
@@ -25,7 +25,8 @@ rmw_ret_t
 get_node_names(
   const char * implementation_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_CONNEXT_SHARED_CPP_PUBLIC
 rmw_ret_t

diff --git a/rmw_connext_shared_cpp/src/node.cpp b/rmw_connext_shared_cpp/src/node.cpp
@@ -55,21 +55,21 @@ create_node(
   participant_qos.participant_name.name = DDS::String_dup(name);
   // since the participant name is not part of the DDS spec
   // the node name is also set in the user_data
-  DDS_Long name_length = static_cast<DDS_Long>(strlen(name));
-  const char prefix[6] = "name=";
-  bool success = participant_qos.user_data.value.length(name_length + sizeof(prefix));
+  size_t length = strlen(name) + strlen("name=;") +
+    strlen(namespace_) + strlen("namespace=;") + 1;
+  bool success = participant_qos.user_data.value.length(static_cast<DDS_Long>(length));
   if (!success) {
     RMW_SET_ERROR_MSG("failed to resize participant user_data");
     return NULL;
   }
-  memcpy(participant_qos.user_data.value.get_contiguous_buffer(), prefix, sizeof(prefix) - 1);
-  {
-    for (DDS_Long i = 0; i < name_length; ++i) {
-      participant_qos.user_data.value[sizeof(prefix) - 1 + i] = name[i];
-    }
-    participant_qos.user_data.value[sizeof(prefix) - 1 + name_length] = ';';
-  }
 
+  int written =
+    snprintf(reinterpret_cast<char *>(participant_qos.user_data.value.get_contiguous_buffer()),
+      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 NULL;
+  }
 
   // Accorinrding to the RTPS spec, ContentFilterProperty_t has the following fields:
   // -contentFilteredTopicName (max length 256)

diff --git a/rmw_connext_shared_cpp/src/node_names.cpp b/rmw_connext_shared_cpp/src/node_names.cpp
@@ -31,7 +31,8 @@ rmw_ret_t
 get_node_names(
   const char * implementation_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("node handle is null");
@@ -58,6 +59,11 @@ get_node_names(
     RMW_SET_ERROR_MSG(rcutils_get_error_string_safe())
     return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret);
   }
+  rcutils_ret = rcutils_string_array_init(node_namespaces, length, &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);
+  }
 
   DDS_DomainParticipantQos participant_qos;
   DDS_ReturnCode_t status = participant->get_qos(participant_qos);
@@ -70,18 +76,29 @@ get_node_names(
     RMW_SET_ERROR_MSG("could not allocate memory for node name")
     return RMW_RET_BAD_ALLOC;
   }
+  node_namespaces->data[0] = rcutils_strdup(node->namespace_, allocator);
+  if (!node_names->data[0]) {
+    RMW_SET_ERROR_MSG("could not allocate memory for node namespace")
+    return RMW_RET_BAD_ALLOC;
+  }
 
   for (auto i = 1; i < length; ++i) {
     DDS::ParticipantBuiltinTopicData pbtd;
     auto dds_ret = participant->get_discovered_participant_data(pbtd, handles[i - 1]);
     std::string name;
+    std::string namespace_;
     if (DDS_RETCODE_OK == dds_ret) {
       auto data = static_cast<unsigned char *>(pbtd.user_data.value.get_contiguous_buffer());
       std::vector<uint8_t> kv(data, data + pbtd.user_data.value.length());
       auto map = rmw::impl::cpp::parse_key_value(kv);
-      auto found = map.find("name");
-      if (found != map.end()) {
-        name = std::string(found->second.begin(), found->second.end());
+      auto name_found = map.find("name");
+      auto ns_found = map.find("namespace");
+
+      if (name_found != map.end()) {
+        name = std::string(name_found->second.begin(), name_found->second.end());
+      }
+      if (name_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
@@ -93,20 +110,42 @@ get_node_names(
     if (name.empty()) {
       // ignore discovered participants without a name
       node_names->data[i] = nullptr;
+      node_namespaces->data[i] = nullptr;
       continue;
     }
+
     node_names->data[i] = rcutils_strdup(name.c_str(), allocator);
     if (!node_names->data[i]) {
       RMW_SET_ERROR_MSG("could not allocate memory for node name")
-      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())
-      }
-      return RMW_RET_BAD_ALLOC;
+      goto fail;
+    }
+
+    node_namespaces->data[i] = rcutils_strdup(namespace_.c_str(), allocator);
+    if (!node_namespaces->data[i]) {
+      RMW_SET_ERROR_MSG("could not allocate memory for node namespace")
+      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;
 }