From 4ba2761758fcacab2e3fc80421f349c2af1eeae1 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Thu, 7 Dec 2017 14:20:09 -0800 Subject: [PATCH 1/2] add node name to user data --- rmw_fastrtps_cpp/src/rmw_node.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index 6f2a404f2..381c7e729 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -217,7 +217,17 @@ rmw_create_node( Domain::getDefaultParticipantAttributes(participantAttrs); participantAttrs.rtps.builtin.domainId = static_cast(domain_id); + // since the participant name is not part of the DDS spec participantAttrs.rtps.setName(name); + // 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]; + } + 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 From 252bee5e70f4c475c21b2d5a5441e0028b684acb Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 15 Dec 2017 18:27:43 +0100 Subject: [PATCH 2/2] add participant listener --- .../custom_participant_info.hpp | 63 +++++++++++++++++++ rmw_fastrtps_cpp/src/rmw_node.cpp | 19 +++++- rmw_fastrtps_cpp/src/rmw_node_names.cpp | 13 ++-- 3 files changed, 87 insertions(+), 8 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_participant_info.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_participant_info.hpp index 4335cf504..804aea487 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_participant_info.hpp @@ -15,19 +15,82 @@ #ifndef RMW_FASTRTPS_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ #define RMW_FASTRTPS_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ +#include +#include +#include + +#include "fastrtps/attributes/ParticipantAttributes.h" #include "fastrtps/participant/Participant.h" +#include "fastrtps/participant/ParticipantListener.h" +#include "rmw/impl/cpp/key_value.hpp" #include "rmw/rmw.h" #include "rmw_fastrtps_cpp/reader_info.hpp" #include "rmw_fastrtps_cpp/writer_info.hpp" +class ParticipantListener; + typedef struct CustomParticipantInfo { eprosima::fastrtps::Participant * participant; + ::ParticipantListener * listener; ReaderInfo * secondarySubListener; WriterInfo * secondaryPubListener; rmw_guard_condition_t * graph_guard_condition; } CustomParticipantInfo; +class ParticipantListener : public eprosima::fastrtps::ParticipantListener +{ +public: + void onParticipantDiscovery(Participant *, ParticipantDiscoveryInfo info) override + { + if ( + info.rtps.m_status != DISCOVERED_RTPSPARTICIPANT && + info.rtps.m_status != REMOVED_RTPSPARTICIPANT && + info.rtps.m_status != DROPPED_RTPSPARTICIPANT) + { + return; + } + + if (DISCOVERED_RTPSPARTICIPANT == info.rtps.m_status) { + // 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"); + std::string name; + if (found != map.end()) { + name = std::string(found->second.begin(), 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; + } + } + } else { + auto it = discovered_names.find(info.rtps.m_guid); + // only consider known GUIDs + if (it != discovered_names.end()) { + discovered_names.erase(it); + } + } + } + + std::vector get_discovered_names() const + { + std::vector names(discovered_names.size()); + size_t i = 0; + for (auto it : discovered_names) { + names[i++] = it.second; + } + return names; + } + + std::map discovered_names; +}; + #endif // RMW_FASTRTPS_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index 381c7e729..a7b35f11e 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -65,6 +65,8 @@ create_node( } // Declare everything before beginning to create things. + ::ParticipantListener * listener = nullptr; + Participant * participant = nullptr; rmw_guard_condition_t * graph_guard_condition = nullptr; CustomParticipantInfo * node_impl = nullptr; rmw_node_t * node_handle = nullptr; @@ -72,7 +74,14 @@ create_node( WriterInfo * tnat_2 = nullptr; std::pair edp_readers; - Participant * participant = Domain::createParticipant(participantAttrs); + try { + listener = new ::ParticipantListener(); + } catch (std::bad_alloc &) { + RMW_SET_ERROR_MSG("failed to allocate participant listener"); + goto fail; + } + + participant = Domain::createParticipant(participantAttrs, listener); if (!participant) { RMW_SET_ERROR_MSG("create_node() could not create participant"); return nullptr; @@ -98,6 +107,7 @@ create_node( } node_handle->implementation_identifier = eprosima_fastrtps_identifier; node_impl->participant = participant; + node_impl->listener = listener; node_impl->graph_guard_condition = graph_guard_condition; node_handle->data = node_impl; @@ -160,6 +170,7 @@ create_node( "failed to destroy guard condition during error handling") } } + rmw_free(listener); if (participant) { Domain::removeParticipant(participant); } @@ -317,10 +328,12 @@ rmw_destroy_node(rmw_node_t * node) result_ret = RMW_RET_ERROR; } - delete impl; - Domain::removeParticipant(participant); + delete impl->listener; + impl->listener = nullptr; + delete impl; + return result_ret; } diff --git a/rmw_fastrtps_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_cpp/src/rmw_node_names.cpp index b99468263..40afa7a53 100644 --- a/rmw_fastrtps_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node_names.cpp @@ -51,18 +51,21 @@ rmw_get_node_names( } auto impl = static_cast(node->data); - Participant * participant = impl->participant; + auto participant_names = impl->listener->get_discovered_names(); - auto participant_names = participant->getParticipantNames(); rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcutils_ret_t rcutils_ret = - rcutils_string_array_init(node_names, participant_names.size(), &allocator); + 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); } - for (size_t i = 0; i < participant_names.size(); ++i) { - node_names->data[i] = rcutils_strdup(participant_names[i].c_str(), allocator); + for (size_t i = 0; i < participant_names.size() + 1; ++i) { + if (0 == i) { + node_names->data[i] = rcutils_strdup(node->name, allocator); + } else { + node_names->data[i] = rcutils_strdup(participant_names[i - 1].c_str(), allocator); + } if (!node_names->data[i]) { RMW_SET_ERROR_MSG("failed to allocate memory for node name") rcutils_ret = rcutils_string_array_fini(node_names);