From 2847a2ebc3bdc0760837c3b86f2ff4f48e86592d Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Fri, 9 Jul 2021 13:13:33 +0200 Subject: [PATCH 1/3] Fix graph manager datawriters Fixes Fix Fix destruction --- .../agent/graph_manager/graph_manager.hpp | 37 +++--- micro_ros_agent/src/agent/Agent.cpp | 105 +++++++----------- .../src/agent/graph_manager/graph_manager.cpp | 98 +++++++++------- 3 files changed, 119 insertions(+), 121 deletions(-) diff --git a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp index 1ef6e41..e929df2 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -89,28 +89,22 @@ class GraphManager /** * @brief Adds a DDS participant to the graph tree. - * @param participant Pointer to the participant to be added to the graph. - */ - void add_participant( - const eprosima::fastdds::dds::DomainParticipant* participant); - - /** - * @brief Adds a DDS participant to the graph tree. - * @param guid rtps::GUID_t of the participant to be added. - * @param node_name Name of the ROS 2 node associated to the given participant. + * @param participant eprosima::fastdds::dds::DomainParticipant to be added. + * @param from_microros if this participant has been added from micro-ROS. * @param enclave ROS 2 enclave. */ void add_participant( - const eprosima::fastrtps::rtps::GUID_t& guid, - const std::string& node_name, - const std::string& enclave); + eprosima::fastdds::dds::DomainParticipant* participant, + bool from_microros = true, + const std::string& enclave = "/"); /** * @brief Removes a DDS participant from the graph tree. - * @param guid rtps::GUID_t of the participant to be removed. + * @param participant eprosima::fastdds::dds::DomainParticipant to be removed. */ void remove_participant( - const eprosima::fastrtps::rtps::GUID_t& guid); + eprosima::fastdds::dds::DomainParticipant* participant, + bool from_microros = true); /** * @brief Adds a DDS datawriter to the graph tree. @@ -186,7 +180,7 @@ class GraphManager */ void associate_entity( const eprosima::fastrtps::rtps::GUID_t& entity_guid, - const eprosima::fastdds::dds::DomainParticipant* participant, + eprosima::fastdds::dds::DomainParticipant* participant, const dds::xrce::ObjectKind& entity_kind); private: @@ -277,11 +271,12 @@ class GraphManager eprosima::fastdds::dds::DomainId_t domain_id_; bool graph_changed_; bool display_on_change_; - const char * enclave_; std::thread microros_graph_publisher_; std::mutex mtx_; std::condition_variable cv_; + eprosima::fastdds::dds::DataWriterQos datawriter_qos_; + rmw_dds_common::GraphCache graphCache_; std::unique_ptr participant_listener_; std::unique_ptr datareader_listener_; @@ -293,9 +288,17 @@ class GraphManager std::unique_ptr subscriber_; std::unique_ptr ros_discovery_topic_; std::unique_ptr ros_to_microros_graph_topic_; - std::unique_ptr ros_discovery_datawriter_; std::unique_ptr ros_to_microros_graph_datawriter_; std::unique_ptr ros_discovery_datareader_; + + // Store a auxiliary publishers and datawriter for each participant created in micro-ROS + std::map< + eprosima::fastdds::dds::DomainParticipant*, + std::pair< + std::unique_ptr, + std::unique_ptr + > + > micro_ros_graph_datawriters_; }; } // namespace graph_manager diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 72c4508..54b9203 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -32,20 +32,16 @@ bool Agent::create( bool result = xrce_dds_agent_instance_.create(argc, argv); if (result) { + auto graph_manager_ = find_or_create_graph_manager(0); /** * Add CREATE_PARTICIPANT callback. */ std::function on_create_participant + eprosima::fastdds::dds::DomainParticipant *)> on_create_participant ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void + eprosima::fastdds::dds::DomainParticipant* participant) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); - + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); graph_manager_->add_participant(participant); }); xrce_dds_agent_instance_.add_middleware_callback( @@ -57,17 +53,12 @@ bool Agent::create( * Add REMOVE_PARTICIPANT callback. */ std::function on_delete_participant + eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void + eprosima::fastdds::dds::DomainParticipant* participant) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); - - graph_manager_->remove_participant(participant->guid()); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + graph_manager_->remove_participant(participant); }); xrce_dds_agent_instance_.add_middleware_callback( eprosima::uxr::Middleware::Kind::FASTDDS, @@ -78,22 +69,18 @@ bool Agent::create( * Add CREATE_DATAWRITER callback. */ std::function on_create_datawriter + eprosima::fastdds::dds::DomainParticipant *, + eprosima::fastdds::dds::DataWriter *)> on_create_datawriter ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant, - const eprosima::fastdds::dds::DataWriter* datawriter) -> void + eprosima::fastdds::dds::DomainParticipant* participant, + eprosima::fastdds::dds::DataWriter* datawriter) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datawriter->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + eprosima::fastrtps::rtps::GUID_t datawriter_guid = iHandle2GUID(instance_handle); graph_manager_->add_datawriter(datawriter_guid, participant, datawriter); graph_manager_->associate_entity( @@ -108,23 +95,19 @@ bool Agent::create( * Add DELETE_DATAWRITER callback. */ std::function on_delete_datawriter + eprosima::fastdds::dds::DomainParticipant *, + eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant, - const eprosima::fastdds::dds::DataWriter* datawriter) -> void + eprosima::fastdds::dds::DomainParticipant* participant, + eprosima::fastdds::dds::DataWriter* datawriter) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datawriter->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + eprosima::fastrtps::rtps::GUID_t datawriter_guid = eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); graph_manager_->remove_datawriter(datawriter_guid); }); @@ -138,22 +121,18 @@ bool Agent::create( * Add CREATE_DATAREADER callback. */ std::function on_create_datareader + eprosima::fastdds::dds::DomainParticipant *, + eprosima::fastdds::dds::DataReader*)> on_create_datareader ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant, - const eprosima::fastdds::dds::DataReader* datareader) -> void + eprosima::fastdds::dds::DomainParticipant* participant, + eprosima::fastdds::dds::DataReader* datareader) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datareader->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::GUID_t datareader_guid = eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); graph_manager_->add_datareader(datareader_guid, participant, datareader); graph_manager_->associate_entity( @@ -168,22 +147,18 @@ bool Agent::create( * Add DELETE_DATAREADER callback. */ std::function on_delete_datareader + eprosima::fastdds::dds::DomainParticipant *, + eprosima::fastdds::dds::DataReader *)> on_delete_datareader ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant, - const eprosima::fastdds::dds::DataReader* datareader) -> void + eprosima::fastdds::dds::DomainParticipant* participant, + eprosima::fastdds::dds::DataReader* datareader) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datareader->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::GUID_t datareader_guid = eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); graph_manager_->remove_datareader(datareader_guid); }); @@ -204,16 +179,14 @@ void Agent::run() std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) { - auto it = graph_manager_map_.find(domain_id); + +auto it = graph_manager_map_.find(domain_id); if (it != graph_manager_map_.end()) { return it->second; }else{ return graph_manager_map_.insert( - std::pair< - eprosima::fastdds::dds::DomainId_t, - std::shared_ptr - >( + std::make_pair( domain_id, std::make_shared(domain_id) ) diff --git a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp index c5106b0..dc6573d 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -22,11 +22,9 @@ namespace agent { namespace graph_manager { GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) - // : eprosima::fastrtps::ParticipantListener() : domain_id_(domain_id) , graph_changed_(false) , display_on_change_(false) - , enclave_("/") , mtx_() , cv_() , graphCache_() @@ -43,12 +41,13 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastdds::dds::DomainParticipantQos participant_qos = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos(); - size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave_) + 1; + const char * enclave = "/"; + size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1; participant_qos.user_data().resize(length); snprintf(reinterpret_cast(participant_qos.user_data().data_vec().data()), - length, "enclave=%s;", enclave_); + length, "enclave=%s;", enclave); - participant_qos.name(enclave_); + participant_qos.name(enclave); participant_qos.wire_protocol().builtin.readerHistoryMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; participant_qos.wire_protocol().builtin.writerHistoryMemoryPolicy = @@ -80,28 +79,26 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastdds::dds::TOPIC_QOS_DEFAULT)); // Create datawriters - eprosima::fastdds::dds::DataWriterQos datawriter_qos = + datawriter_qos_ = eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT; - datawriter_qos.history().kind = + datawriter_qos_.history().kind = eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS; - datawriter_qos.history().depth = 1; - datawriter_qos.endpoint().history_memory_policy = + datawriter_qos_.history().depth = 1; + datawriter_qos_.endpoint().history_memory_policy = eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - datawriter_qos.publish_mode().kind = + datawriter_qos_.publish_mode().kind = eprosima::fastdds::dds::PublishModeQosPolicyKind::ASYNCHRONOUS_PUBLISH_MODE; - datawriter_qos.reliability().kind = + datawriter_qos_.reliability().kind = eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS; - datawriter_qos.durability().kind = + datawriter_qos_.durability().kind = eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS; - ros_discovery_datawriter_.reset( - publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos)); - - datawriter_qos.history().kind = + eprosima::fastdds::dds::DataWriterQos ros_to_microros_datawriter_qos_ = datawriter_qos_; + ros_to_microros_datawriter_qos_.history().kind = eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS; ros_to_microros_graph_datawriter_.reset( - publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), datawriter_qos)); + publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), ros_to_microros_datawriter_qos_)); // Create datareaders @@ -295,37 +292,62 @@ inline void GraphManager::publish_microros_graph() } void GraphManager::add_participant( - const eprosima::fastdds::dds::DomainParticipant* participant) -{ - const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos(); - this->add_participant(participant->guid(), qos.name().to_string(), enclave_); -} - -void GraphManager::add_participant( - const eprosima::fastrtps::rtps::GUID_t& guid, - const std::string& node_name, + eprosima::fastdds::dds::DomainParticipant* participant, + bool from_microros, const std::string& enclave) { - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid); + const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos(); + const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); graphCache_.add_participant(gid, enclave); - if (node_name != enclave) // Do not add root node + // Do not add root node and + // do not announce non-micro-ROS participants + if (qos.name().to_string() != "/" && from_microros) { std::string isolated_node_name, isolated_namespace; - get_name_and_namespace(node_name, isolated_node_name, isolated_namespace); + get_name_and_namespace(qos.name().to_string(), isolated_node_name, isolated_namespace); rmw_dds_common::msg::ParticipantEntitiesInfo info = graphCache_.add_node(gid, isolated_node_name, isolated_namespace); - ros_discovery_datawriter_->write(static_cast(&info)); + + auto it = micro_ros_graph_datawriters_.find(participant); + if (it == micro_ros_graph_datawriters_.end()) + { + // Register participant within typesupport + participant->register_type(*participant_info_typesupport_); + + // Create publisher + std::unique_ptr publisher; + publisher.reset(participant->create_publisher(eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT)); + + // Create datawriter + std::unique_ptr datawriter; + datawriter.reset(publisher->create_datawriter(ros_discovery_topic_.get(), datawriter_qos_)); + + it = micro_ros_graph_datawriters_.insert( + std::make_pair(participant, std::make_pair(std::move(publisher), std::move(datawriter)))).first; + } + + it->second.second->write(static_cast(&info)); } } void GraphManager::remove_participant( - const eprosima::fastrtps::rtps::GUID_t& guid) + eprosima::fastdds::dds::DomainParticipant* participant, + bool from_microros) { - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid); + const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); graphCache_.remove_participant(gid); + + if (from_microros) + { + rmw_dds_common::msg::ParticipantEntitiesInfo info; + rmw_dds_common::convert_gid_to_msg(&gid, &info.gid); + auto it = micro_ros_graph_datawriters_.find(participant); + it->second.second->write(static_cast(&info)); + } + micro_ros_graph_datawriters_.erase(participant); } void GraphManager::add_datawriter( @@ -404,7 +426,7 @@ void GraphManager::remove_datareader( void GraphManager::associate_entity( const eprosima::fastrtps::rtps::GUID_t& entity_guid, - const eprosima::fastdds::dds::DomainParticipant* participant, + eprosima::fastdds::dds::DomainParticipant* participant, const dds::xrce::ObjectKind& entity_kind) { const rmw_gid_t entity_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -438,7 +460,9 @@ void GraphManager::associate_entity( break; } } - ros_discovery_datawriter_->write(static_cast(&info)); + + auto it = micro_ros_graph_datawriters_.find(participant); + it->second.second->write(static_cast(&info)); } @@ -592,13 +616,13 @@ void GraphManager::ParticipantListener::on_participant_discovery( const std::string enclave = std::string(name_found->second.begin(), name_found->second.end()); - graphManager_from_->add_participant(participant->guid(), info.info.m_participantName.to_string(), enclave); + graphManager_from_->add_participant(participant, false, enclave); break; } case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: { - graphManager_from_->remove_participant(info.info.m_guid); + graphManager_from_->remove_participant(participant, false); break; } default: @@ -713,7 +737,6 @@ void GraphManager::ParticipantListener::on_subscriber_discovery( eprosima::fastrtps::rtps::ReaderDiscoveryInfo&& info) { process_discovery_info(info); - // graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAREADER); } void GraphManager::ParticipantListener::on_publisher_discovery( @@ -721,7 +744,6 @@ void GraphManager::ParticipantListener::on_publisher_discovery( eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info) { process_discovery_info(info); - // graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAWRITER); } GraphManager::DatareaderListener::DatareaderListener( From 473b3a0dd737fd8fdaa647658893f7ee8fe29756 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Fri, 9 Jul 2021 13:13:33 +0200 Subject: [PATCH 2/3] Revert "Fix graph manager datawriters" This reverts commit 2847a2ebc3bdc0760837c3b86f2ff4f48e86592d. --- .../agent/graph_manager/graph_manager.hpp | 37 +++--- micro_ros_agent/src/agent/Agent.cpp | 105 +++++++++++------- .../src/agent/graph_manager/graph_manager.cpp | 98 +++++++--------- 3 files changed, 121 insertions(+), 119 deletions(-) diff --git a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp index e929df2..1ef6e41 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -89,22 +89,28 @@ class GraphManager /** * @brief Adds a DDS participant to the graph tree. - * @param participant eprosima::fastdds::dds::DomainParticipant to be added. - * @param from_microros if this participant has been added from micro-ROS. + * @param participant Pointer to the participant to be added to the graph. + */ + void add_participant( + const eprosima::fastdds::dds::DomainParticipant* participant); + + /** + * @brief Adds a DDS participant to the graph tree. + * @param guid rtps::GUID_t of the participant to be added. + * @param node_name Name of the ROS 2 node associated to the given participant. * @param enclave ROS 2 enclave. */ void add_participant( - eprosima::fastdds::dds::DomainParticipant* participant, - bool from_microros = true, - const std::string& enclave = "/"); + const eprosima::fastrtps::rtps::GUID_t& guid, + const std::string& node_name, + const std::string& enclave); /** * @brief Removes a DDS participant from the graph tree. - * @param participant eprosima::fastdds::dds::DomainParticipant to be removed. + * @param guid rtps::GUID_t of the participant to be removed. */ void remove_participant( - eprosima::fastdds::dds::DomainParticipant* participant, - bool from_microros = true); + const eprosima::fastrtps::rtps::GUID_t& guid); /** * @brief Adds a DDS datawriter to the graph tree. @@ -180,7 +186,7 @@ class GraphManager */ void associate_entity( const eprosima::fastrtps::rtps::GUID_t& entity_guid, - eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DomainParticipant* participant, const dds::xrce::ObjectKind& entity_kind); private: @@ -271,12 +277,11 @@ class GraphManager eprosima::fastdds::dds::DomainId_t domain_id_; bool graph_changed_; bool display_on_change_; + const char * enclave_; std::thread microros_graph_publisher_; std::mutex mtx_; std::condition_variable cv_; - eprosima::fastdds::dds::DataWriterQos datawriter_qos_; - rmw_dds_common::GraphCache graphCache_; std::unique_ptr participant_listener_; std::unique_ptr datareader_listener_; @@ -288,17 +293,9 @@ class GraphManager std::unique_ptr subscriber_; std::unique_ptr ros_discovery_topic_; std::unique_ptr ros_to_microros_graph_topic_; + std::unique_ptr ros_discovery_datawriter_; std::unique_ptr ros_to_microros_graph_datawriter_; std::unique_ptr ros_discovery_datareader_; - - // Store a auxiliary publishers and datawriter for each participant created in micro-ROS - std::map< - eprosima::fastdds::dds::DomainParticipant*, - std::pair< - std::unique_ptr, - std::unique_ptr - > - > micro_ros_graph_datawriters_; }; } // namespace graph_manager diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 54b9203..72c4508 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -32,16 +32,20 @@ bool Agent::create( bool result = xrce_dds_agent_instance_.create(argc, argv); if (result) { - auto graph_manager_ = find_or_create_graph_manager(0); /** * Add CREATE_PARTICIPANT callback. */ std::function on_create_participant + const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant ([&]( - eprosima::fastdds::dds::DomainParticipant* participant) -> void + const eprosima::fastdds::dds::DomainParticipant* participant) -> void { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + auto graph_manager_ = + find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( + participant->get_domain_id() + ) + ); + graph_manager_->add_participant(participant); }); xrce_dds_agent_instance_.add_middleware_callback( @@ -53,12 +57,17 @@ bool Agent::create( * Add REMOVE_PARTICIPANT callback. */ std::function on_delete_participant + const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant ([&]( - eprosima::fastdds::dds::DomainParticipant* participant) -> void + const eprosima::fastdds::dds::DomainParticipant* participant) -> void { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - graph_manager_->remove_participant(participant); + auto graph_manager_ = + find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( + participant->get_domain_id() + ) + ); + + graph_manager_->remove_participant(participant->guid()); }); xrce_dds_agent_instance_.add_middleware_callback( eprosima::uxr::Middleware::Kind::FASTDDS, @@ -69,18 +78,22 @@ bool Agent::create( * Add CREATE_DATAWRITER callback. */ std::function on_create_datawriter + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter ([&]( - eprosima::fastdds::dds::DomainParticipant* participant, - eprosima::fastdds::dds::DataWriter* datawriter) -> void + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataWriter* datawriter) -> void { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + auto graph_manager_ = + find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( + participant->get_domain_id() + ) + ); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datawriter->get_instance_handle(); - eprosima::fastrtps::rtps::GUID_t datawriter_guid = + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = iHandle2GUID(instance_handle); graph_manager_->add_datawriter(datawriter_guid, participant, datawriter); graph_manager_->associate_entity( @@ -95,19 +108,23 @@ bool Agent::create( * Add DELETE_DATAWRITER callback. */ std::function on_delete_datawriter + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter ([&]( - eprosima::fastdds::dds::DomainParticipant* participant, - eprosima::fastdds::dds::DataWriter* datawriter) -> void + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataWriter* datawriter) -> void { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + auto graph_manager_ = + find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( + participant->get_domain_id() + ) + ); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datawriter->get_instance_handle(); - eprosima::fastrtps::rtps::GUID_t datawriter_guid = + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); graph_manager_->remove_datawriter(datawriter_guid); }); @@ -121,18 +138,22 @@ bool Agent::create( * Add CREATE_DATAREADER callback. */ std::function on_create_datareader + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataReader*)> on_create_datareader ([&]( - eprosima::fastdds::dds::DomainParticipant* participant, - eprosima::fastdds::dds::DataReader* datareader) -> void + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataReader* datareader) -> void { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + auto graph_manager_ = + find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( + participant->get_domain_id() + ) + ); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datareader->get_instance_handle(); - eprosima::fastrtps::rtps::GUID_t datareader_guid = + const eprosima::fastrtps::rtps::GUID_t datareader_guid = eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); graph_manager_->add_datareader(datareader_guid, participant, datareader); graph_manager_->associate_entity( @@ -147,18 +168,22 @@ bool Agent::create( * Add DELETE_DATAREADER callback. */ std::function on_delete_datareader + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataReader *)> on_delete_datareader ([&]( - eprosima::fastdds::dds::DomainParticipant* participant, - eprosima::fastdds::dds::DataReader* datareader) -> void + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataReader* datareader) -> void { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + auto graph_manager_ = + find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( + participant->get_domain_id() + ) + ); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datareader->get_instance_handle(); - eprosima::fastrtps::rtps::GUID_t datareader_guid = + const eprosima::fastrtps::rtps::GUID_t datareader_guid = eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); graph_manager_->remove_datareader(datareader_guid); }); @@ -179,14 +204,16 @@ void Agent::run() std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) { - -auto it = graph_manager_map_.find(domain_id); + auto it = graph_manager_map_.find(domain_id); if (it != graph_manager_map_.end()) { return it->second; }else{ return graph_manager_map_.insert( - std::make_pair( + std::pair< + eprosima::fastdds::dds::DomainId_t, + std::shared_ptr + >( domain_id, std::make_shared(domain_id) ) diff --git a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp index dc6573d..c5106b0 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -22,9 +22,11 @@ namespace agent { namespace graph_manager { GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) + // : eprosima::fastrtps::ParticipantListener() : domain_id_(domain_id) , graph_changed_(false) , display_on_change_(false) + , enclave_("/") , mtx_() , cv_() , graphCache_() @@ -41,13 +43,12 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastdds::dds::DomainParticipantQos participant_qos = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos(); - const char * enclave = "/"; - size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1; + size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave_) + 1; participant_qos.user_data().resize(length); snprintf(reinterpret_cast(participant_qos.user_data().data_vec().data()), - length, "enclave=%s;", enclave); + length, "enclave=%s;", enclave_); - participant_qos.name(enclave); + participant_qos.name(enclave_); participant_qos.wire_protocol().builtin.readerHistoryMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; participant_qos.wire_protocol().builtin.writerHistoryMemoryPolicy = @@ -79,26 +80,28 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastdds::dds::TOPIC_QOS_DEFAULT)); // Create datawriters - datawriter_qos_ = + eprosima::fastdds::dds::DataWriterQos datawriter_qos = eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT; - datawriter_qos_.history().kind = + datawriter_qos.history().kind = eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS; - datawriter_qos_.history().depth = 1; - datawriter_qos_.endpoint().history_memory_policy = + datawriter_qos.history().depth = 1; + datawriter_qos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - datawriter_qos_.publish_mode().kind = + datawriter_qos.publish_mode().kind = eprosima::fastdds::dds::PublishModeQosPolicyKind::ASYNCHRONOUS_PUBLISH_MODE; - datawriter_qos_.reliability().kind = + datawriter_qos.reliability().kind = eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS; - datawriter_qos_.durability().kind = + datawriter_qos.durability().kind = eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS; - eprosima::fastdds::dds::DataWriterQos ros_to_microros_datawriter_qos_ = datawriter_qos_; - ros_to_microros_datawriter_qos_.history().kind = + ros_discovery_datawriter_.reset( + publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos)); + + datawriter_qos.history().kind = eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS; ros_to_microros_graph_datawriter_.reset( - publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), ros_to_microros_datawriter_qos_)); + publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), datawriter_qos)); // Create datareaders @@ -292,62 +295,37 @@ inline void GraphManager::publish_microros_graph() } void GraphManager::add_participant( - eprosima::fastdds::dds::DomainParticipant* participant, - bool from_microros, - const std::string& enclave) + const eprosima::fastdds::dds::DomainParticipant* participant) { const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos(); - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); + this->add_participant(participant->guid(), qos.name().to_string(), enclave_); +} + +void GraphManager::add_participant( + const eprosima::fastrtps::rtps::GUID_t& guid, + const std::string& node_name, + const std::string& enclave) +{ + const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid); graphCache_.add_participant(gid, enclave); - // Do not add root node and - // do not announce non-micro-ROS participants - if (qos.name().to_string() != "/" && from_microros) + if (node_name != enclave) // Do not add root node { std::string isolated_node_name, isolated_namespace; - get_name_and_namespace(qos.name().to_string(), isolated_node_name, isolated_namespace); + get_name_and_namespace(node_name, isolated_node_name, isolated_namespace); rmw_dds_common::msg::ParticipantEntitiesInfo info = graphCache_.add_node(gid, isolated_node_name, isolated_namespace); - - auto it = micro_ros_graph_datawriters_.find(participant); - if (it == micro_ros_graph_datawriters_.end()) - { - // Register participant within typesupport - participant->register_type(*participant_info_typesupport_); - - // Create publisher - std::unique_ptr publisher; - publisher.reset(participant->create_publisher(eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT)); - - // Create datawriter - std::unique_ptr datawriter; - datawriter.reset(publisher->create_datawriter(ros_discovery_topic_.get(), datawriter_qos_)); - - it = micro_ros_graph_datawriters_.insert( - std::make_pair(participant, std::make_pair(std::move(publisher), std::move(datawriter)))).first; - } - - it->second.second->write(static_cast(&info)); + ros_discovery_datawriter_->write(static_cast(&info)); } } void GraphManager::remove_participant( - eprosima::fastdds::dds::DomainParticipant* participant, - bool from_microros) + const eprosima::fastrtps::rtps::GUID_t& guid) { - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); + const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid); graphCache_.remove_participant(gid); - - if (from_microros) - { - rmw_dds_common::msg::ParticipantEntitiesInfo info; - rmw_dds_common::convert_gid_to_msg(&gid, &info.gid); - auto it = micro_ros_graph_datawriters_.find(participant); - it->second.second->write(static_cast(&info)); - } - micro_ros_graph_datawriters_.erase(participant); } void GraphManager::add_datawriter( @@ -426,7 +404,7 @@ void GraphManager::remove_datareader( void GraphManager::associate_entity( const eprosima::fastrtps::rtps::GUID_t& entity_guid, - eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DomainParticipant* participant, const dds::xrce::ObjectKind& entity_kind) { const rmw_gid_t entity_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -460,9 +438,7 @@ void GraphManager::associate_entity( break; } } - - auto it = micro_ros_graph_datawriters_.find(participant); - it->second.second->write(static_cast(&info)); + ros_discovery_datawriter_->write(static_cast(&info)); } @@ -616,13 +592,13 @@ void GraphManager::ParticipantListener::on_participant_discovery( const std::string enclave = std::string(name_found->second.begin(), name_found->second.end()); - graphManager_from_->add_participant(participant, false, enclave); + graphManager_from_->add_participant(participant->guid(), info.info.m_participantName.to_string(), enclave); break; } case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: { - graphManager_from_->remove_participant(participant, false); + graphManager_from_->remove_participant(info.info.m_guid); break; } default: @@ -737,6 +713,7 @@ void GraphManager::ParticipantListener::on_subscriber_discovery( eprosima::fastrtps::rtps::ReaderDiscoveryInfo&& info) { process_discovery_info(info); + // graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAREADER); } void GraphManager::ParticipantListener::on_publisher_discovery( @@ -744,6 +721,7 @@ void GraphManager::ParticipantListener::on_publisher_discovery( eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info) { process_discovery_info(info); + // graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAWRITER); } GraphManager::DatareaderListener::DatareaderListener( From a6db73f76ad841981c741b1f2c3bda7ddf973af9 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Fri, 9 Jul 2021 13:23:13 +0200 Subject: [PATCH 3/3] Initial --- .../agent/graph_manager/graph_manager.hpp | 32 +++---- micro_ros_agent/src/agent/Agent.cpp | 48 +++------- .../src/agent/graph_manager/graph_manager.cpp | 89 +++++++++++-------- 3 files changed, 78 insertions(+), 91 deletions(-) diff --git a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp index 1ef6e41..f0d94d2 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -89,28 +89,22 @@ class GraphManager /** * @brief Adds a DDS participant to the graph tree. - * @param participant Pointer to the participant to be added to the graph. - */ - void add_participant( - const eprosima::fastdds::dds::DomainParticipant* participant); - - /** - * @brief Adds a DDS participant to the graph tree. - * @param guid rtps::GUID_t of the participant to be added. - * @param node_name Name of the ROS 2 node associated to the given participant. + * @param participant eprosima::fastdds::dds::DomainParticipant to be added. + * @param from_microros if this participant has been added from micro-ROS. * @param enclave ROS 2 enclave. */ void add_participant( - const eprosima::fastrtps::rtps::GUID_t& guid, - const std::string& node_name, - const std::string& enclave); + const eprosima::fastdds::dds::DomainParticipant* participant, + bool from_microros = true, + const std::string& enclave = "/"); /** * @brief Removes a DDS participant from the graph tree. - * @param guid rtps::GUID_t of the participant to be removed. + * @param participant eprosima::fastdds::dds::DomainParticipant to be removed. */ void remove_participant( - const eprosima::fastrtps::rtps::GUID_t& guid); + const eprosima::fastdds::dds::DomainParticipant* participant, + bool from_microros = true); /** * @brief Adds a DDS datawriter to the graph tree. @@ -277,11 +271,12 @@ class GraphManager eprosima::fastdds::dds::DomainId_t domain_id_; bool graph_changed_; bool display_on_change_; - const char * enclave_; std::thread microros_graph_publisher_; std::mutex mtx_; std::condition_variable cv_; + eprosima::fastdds::dds::DataWriterQos datawriter_qos_; + rmw_dds_common::GraphCache graphCache_; std::unique_ptr participant_listener_; std::unique_ptr datareader_listener_; @@ -293,9 +288,14 @@ class GraphManager std::unique_ptr subscriber_; std::unique_ptr ros_discovery_topic_; std::unique_ptr ros_to_microros_graph_topic_; - std::unique_ptr ros_discovery_datawriter_; std::unique_ptr ros_to_microros_graph_datawriter_; std::unique_ptr ros_discovery_datareader_; + + // Store a auxiliary publishers and datawriter for each participant created in micro-ROS + std::map< + const eprosima::fastdds::dds::DomainParticipant*, + std::unique_ptr + > micro_ros_graph_datawriters_; }; } // namespace graph_manager diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 72c4508..87ef3cf 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -40,12 +40,7 @@ bool Agent::create( ([&]( const eprosima::fastdds::dds::DomainParticipant* participant) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); - + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); graph_manager_->add_participant(participant); }); xrce_dds_agent_instance_.add_middleware_callback( @@ -61,13 +56,8 @@ bool Agent::create( ([&]( const eprosima::fastdds::dds::DomainParticipant* participant) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); - - graph_manager_->remove_participant(participant->guid()); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + graph_manager_->remove_participant(participant); }); xrce_dds_agent_instance_.add_middleware_callback( eprosima::uxr::Middleware::Kind::FASTDDS, @@ -84,11 +74,7 @@ bool Agent::create( const eprosima::fastdds::dds::DomainParticipant* participant, const eprosima::fastdds::dds::DataWriter* datawriter) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = @@ -115,11 +101,7 @@ bool Agent::create( const eprosima::fastdds::dds::DataWriter* datawriter) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = @@ -144,11 +126,7 @@ bool Agent::create( const eprosima::fastdds::dds::DomainParticipant* participant, const eprosima::fastdds::dds::DataReader* datareader) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = @@ -174,11 +152,7 @@ bool Agent::create( const eprosima::fastdds::dds::DomainParticipant* participant, const eprosima::fastdds::dds::DataReader* datareader) -> void { - auto graph_manager_ = - find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant->get_domain_id() - ) - ); + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = @@ -204,16 +178,14 @@ void Agent::run() std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) { - auto it = graph_manager_map_.find(domain_id); + +auto it = graph_manager_map_.find(domain_id); if (it != graph_manager_map_.end()) { return it->second; }else{ return graph_manager_map_.insert( - std::pair< - eprosima::fastdds::dds::DomainId_t, - std::shared_ptr - >( + std::make_pair( domain_id, std::make_shared(domain_id) ) diff --git a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp index c5106b0..9badffc 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -22,11 +22,9 @@ namespace agent { namespace graph_manager { GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) - // : eprosima::fastrtps::ParticipantListener() : domain_id_(domain_id) , graph_changed_(false) , display_on_change_(false) - , enclave_("/") , mtx_() , cv_() , graphCache_() @@ -43,12 +41,13 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastdds::dds::DomainParticipantQos participant_qos = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos(); - size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave_) + 1; + const char * enclave = "/"; + size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1; participant_qos.user_data().resize(length); snprintf(reinterpret_cast(participant_qos.user_data().data_vec().data()), - length, "enclave=%s;", enclave_); + length, "enclave=%s;", enclave); - participant_qos.name(enclave_); + participant_qos.name(enclave); participant_qos.wire_protocol().builtin.readerHistoryMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; participant_qos.wire_protocol().builtin.writerHistoryMemoryPolicy = @@ -80,28 +79,26 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastdds::dds::TOPIC_QOS_DEFAULT)); // Create datawriters - eprosima::fastdds::dds::DataWriterQos datawriter_qos = + datawriter_qos_ = eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT; - datawriter_qos.history().kind = + datawriter_qos_.history().kind = eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS; - datawriter_qos.history().depth = 1; - datawriter_qos.endpoint().history_memory_policy = + datawriter_qos_.history().depth = 1; + datawriter_qos_.endpoint().history_memory_policy = eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - datawriter_qos.publish_mode().kind = + datawriter_qos_.publish_mode().kind = eprosima::fastdds::dds::PublishModeQosPolicyKind::ASYNCHRONOUS_PUBLISH_MODE; - datawriter_qos.reliability().kind = + datawriter_qos_.reliability().kind = eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS; - datawriter_qos.durability().kind = + datawriter_qos_.durability().kind = eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS; - ros_discovery_datawriter_.reset( - publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos)); - - datawriter_qos.history().kind = + eprosima::fastdds::dds::DataWriterQos ros_to_microros_datawriter_qos_ = datawriter_qos_; + ros_to_microros_datawriter_qos_.history().kind = eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS; ros_to_microros_graph_datawriter_.reset( - publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), datawriter_qos)); + publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), ros_to_microros_datawriter_qos_)); // Create datareaders @@ -295,37 +292,55 @@ inline void GraphManager::publish_microros_graph() } void GraphManager::add_participant( - const eprosima::fastdds::dds::DomainParticipant* participant) -{ - const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos(); - this->add_participant(participant->guid(), qos.name().to_string(), enclave_); -} - -void GraphManager::add_participant( - const eprosima::fastrtps::rtps::GUID_t& guid, - const std::string& node_name, + const eprosima::fastdds::dds::DomainParticipant* participant, + bool from_microros, const std::string& enclave) { - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid); + const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos(); + const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); graphCache_.add_participant(gid, enclave); - if (node_name != enclave) // Do not add root node + // Do not add root node and + // do not announce non-micro-ROS participants + if (qos.name().to_string() != "/" && from_microros) { std::string isolated_node_name, isolated_namespace; - get_name_and_namespace(node_name, isolated_node_name, isolated_namespace); + get_name_and_namespace(qos.name().to_string(), isolated_node_name, isolated_namespace); rmw_dds_common::msg::ParticipantEntitiesInfo info = graphCache_.add_node(gid, isolated_node_name, isolated_namespace); - ros_discovery_datawriter_->write(static_cast(&info)); + + auto it = micro_ros_graph_datawriters_.find(participant); + if (it == micro_ros_graph_datawriters_.end()) + { + // Create datawriter + std::unique_ptr datawriter; + datawriter.reset(publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos_)); + + it = micro_ros_graph_datawriters_.insert( + std::make_pair(participant, std::move(datawriter))).first; + } + + it->second->write(static_cast(&info)); } } void GraphManager::remove_participant( - const eprosima::fastrtps::rtps::GUID_t& guid) + const eprosima::fastdds::dds::DomainParticipant* participant, + bool from_microros) { - const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid); + const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid()); graphCache_.remove_participant(gid); + + if (from_microros) + { + rmw_dds_common::msg::ParticipantEntitiesInfo info; + rmw_dds_common::convert_gid_to_msg(&gid, &info.gid); + auto it = micro_ros_graph_datawriters_.find(participant); + it->second->write(static_cast(&info)); + } + micro_ros_graph_datawriters_.erase(participant); } void GraphManager::add_datawriter( @@ -438,7 +453,9 @@ void GraphManager::associate_entity( break; } } - ros_discovery_datawriter_->write(static_cast(&info)); + + auto it = micro_ros_graph_datawriters_.find(participant); + it->second->write(static_cast(&info)); } @@ -592,13 +609,13 @@ void GraphManager::ParticipantListener::on_participant_discovery( const std::string enclave = std::string(name_found->second.begin(), name_found->second.end()); - graphManager_from_->add_participant(participant->guid(), info.info.m_participantName.to_string(), enclave); + graphManager_from_->add_participant(participant, false, enclave); break; } case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: { - graphManager_from_->remove_participant(info.info.m_guid); + graphManager_from_->remove_participant(participant, false); break; } default: @@ -713,7 +730,6 @@ void GraphManager::ParticipantListener::on_subscriber_discovery( eprosima::fastrtps::rtps::ReaderDiscoveryInfo&& info) { process_discovery_info(info); - // graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAREADER); } void GraphManager::ParticipantListener::on_publisher_discovery( @@ -721,7 +737,6 @@ void GraphManager::ParticipantListener::on_publisher_discovery( eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info) { process_discovery_info(info); - // graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAWRITER); } GraphManager::DatareaderListener::DatareaderListener(