diff --git a/rmw_connext_cpp/src/rmw_node_names.cpp b/rmw_connext_cpp/src/rmw_node_names.cpp index 676d6724..3226619f 100644 --- 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 index 02845681..8c2f5516 100644 --- 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 index a4c23566..0e21b52b 100644 --- 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(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(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(participant_qos.user_data.value.get_contiguous_buffer()), + length, "name=%s;namespace=%s;", name, namespace_); + if (written < 0 || written > static_cast(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 index f7c7ffe2..8723be47 100644 --- 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(pbtd.user_data.value.get_contiguous_buffer()); std::vector 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; }