diff --git a/rmw_fastrtps_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_cpp/src/rmw_node_names.cpp index 9db14f9e5..bcdbe6648 100644 --- 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 index f957ae4c6..55440d33a 100644 --- 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 index 775739875..4e086435d 100644 --- 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,11 +58,19 @@ 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; @@ -70,13 +78,23 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener // 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 get_discovered_namespaces() const + { + std::vector namespaces(discovered_namespaces.size()); + size_t i = 0; + for (auto it : discovered_namespaces) { + namespaces[i++] = it.second; + } + return namespaces; + } + + std::map discovered_names; + std::map 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 index 59d6e90c1..44c06a77e 100644 --- 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 index 6d06ef1e0..83eec1bd1 100644 --- 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(participantAttrs.rtps.userData.data()), + 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 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 index 9bb5d64c2..6008d86bc 100644 --- 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(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