From e832f3fc87c47c0ba25a9362b4c7a1224b93a532 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 21 Jun 2021 15:14:34 +0200 Subject: [PATCH 1/5] Add multi domain graph manager --- micro_ros_agent/include/agent/Agent.hpp | 4 +- .../agent/graph_manager/graph_manager.hpp | 5 +- micro_ros_agent/src/agent/Agent.cpp | 60 ++++++++++++++++++- .../src/agent/graph_manager/graph_manager.cpp | 9 ++- 4 files changed, 67 insertions(+), 11 deletions(-) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index ca351b5..621eeff 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -41,7 +41,9 @@ class Agent private: eprosima::uxr::AgentInstance& xrce_dds_agent_instance_; - std::unique_ptr graph_manager_; + std::map> graph_manager_map_; + + std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id); }; } // namespace agent 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 ca0e23f..e141095 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -75,10 +75,10 @@ class GraphManager /** * @brief Default constructor. */ - GraphManager(); + GraphManager(eprosima::fastdds::dds::DomainId_t domain_id); /** - * @brief Default destructor. + * @brief Default destrucctor. */ ~GraphManager() = default; @@ -292,6 +292,7 @@ class GraphManager */ void update_node_entities_info(); + eprosima::fastdds::dds::DomainId_t domain_id_; bool graph_changed_; bool display_on_change_; const char * enclave_; diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 79f9d9e..d103581 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -33,8 +33,6 @@ bool Agent::create( bool result = xrce_dds_agent_instance_.create(argc, argv); if (result) { - graph_manager_.reset(new graph_manager::GraphManager()); - /** * Add CREATE_PARTICIPANT callback. */ @@ -43,6 +41,12 @@ 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_->add_participant(participant); }); xrce_dds_agent_instance_.add_middleware_callback( @@ -58,6 +62,12 @@ 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()); }); xrce_dds_agent_instance_.add_middleware_callback( @@ -75,6 +85,12 @@ 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() + ) + ); + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datawriter->get_instance_handle(); @@ -99,6 +115,13 @@ 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() + ) + ); + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datawriter->get_instance_handle(); @@ -122,6 +145,12 @@ 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() + ) + ); + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datareader->get_instance_handle(); @@ -143,9 +172,15 @@ bool Agent::create( const eprosima::fastdds::dds::DomainParticipant *, const eprosima::fastdds::dds::DataReader *)> on_delete_datareader ([&]( - const eprosima::fastdds::dds::DomainParticipant* /*participant*/, + 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() + ) + ); + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = datareader->get_instance_handle(); @@ -168,6 +203,25 @@ void Agent::run() return xrce_dds_agent_instance_.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); + + 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 + >( + domain_id, + std::make_shared(domain_id) + ) + ); + } +} + } // namespace agent } // namespace uros #endif // _UROS_AGENT_AGENT_CPP \ No newline at end of file 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 8432f6c..8993576 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -21,9 +21,10 @@ namespace uros { namespace agent { namespace graph_manager { -GraphManager::GraphManager() +GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) // : eprosima::fastrtps::ParticipantListener() - : graph_changed_(false) + : domain_id_(domain_id) + , graph_changed_(false) , display_on_change_(false) , enclave_("/") , mtx_() @@ -37,8 +38,6 @@ GraphManager::GraphManager() eprosima::fastdds::dds::TypeSupport>(new graph_manager::MicrorosGraphInfoTypeSupport())) { // Create DomainParticipant - eprosima::fastdds::dds::DomainId_t domain_id(0); - eprosima::fastdds::dds::DomainParticipantQos participant_qos = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos(); @@ -54,7 +53,7 @@ GraphManager::GraphManager() eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; participant_.reset(eprosima::fastdds::dds::DomainParticipantFactory::get_instance()-> - create_participant(domain_id, participant_qos, participant_listener_.get())); + create_participant(domain_id_, participant_qos, participant_listener_.get())); // Register participant within typesupport participant_->register_type(*participant_info_typesupport_); From fd72bb326736c6fc3e61ef0812a05aef84cd7a4e Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 21 Jun 2021 15:16:56 +0200 Subject: [PATCH 2/5] Update --- micro_ros_agent/include/agent/Agent.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index 621eeff..f5f22f3 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -43,7 +43,7 @@ class Agent eprosima::uxr::AgentInstance& xrce_dds_agent_instance_; std::map> graph_manager_map_; - std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id); + std::shared_ptr find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id); }; } // namespace agent From 98502c104d690427e06b3a313ec72eda022e2b85 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 21 Jun 2021 15:31:22 +0200 Subject: [PATCH 3/5] Fix --- micro_ros_agent/src/agent/Agent.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index d103581..ababd26 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -22,7 +22,6 @@ namespace agent { Agent::Agent() : xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance()) - , graph_manager_(nullptr) { } @@ -43,7 +42,7 @@ bool Agent::create( { auto graph_manager_ = find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant.get_domain_id() + participant->get_domain_id() ) ); @@ -64,7 +63,7 @@ bool Agent::create( { auto graph_manager_ = find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant.get_domain_id() + participant->get_domain_id() ) ); @@ -87,7 +86,7 @@ bool Agent::create( { auto graph_manager_ = find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant.get_domain_id() + participant->get_domain_id() ) ); @@ -118,7 +117,7 @@ bool Agent::create( auto graph_manager_ = find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant.get_domain_id() + participant->get_domain_id() ) ); @@ -147,7 +146,7 @@ bool Agent::create( { auto graph_manager_ = find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant.get_domain_id() + participant->get_domain_id() ) ); @@ -177,7 +176,7 @@ bool Agent::create( { auto graph_manager_ = find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( - participant.get_domain_id() + participant->get_domain_id() ) ); @@ -208,7 +207,7 @@ std::shared_ptr Agent::find_or_create_graph_manager auto it = graph_manager_map_.find(domain_id); if (it != graph_manager_map_.end()) { - return it.second(); + return it->second(); }else{ return graph_manager_map_.insert( std::pair< From 769780118ea9b456a3ac4ff6f4eee21834608b40 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 21 Jun 2021 15:51:18 +0200 Subject: [PATCH 4/5] Fix --- micro_ros_agent/src/agent/Agent.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index ababd26..72c4508 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -111,7 +111,7 @@ bool Agent::create( const eprosima::fastdds::dds::DomainParticipant *, const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter ([&]( - const eprosima::fastdds::dds::DomainParticipant* /*participant*/, + const eprosima::fastdds::dds::DomainParticipant* participant, const eprosima::fastdds::dds::DataWriter* datawriter) -> void { @@ -207,7 +207,7 @@ std::shared_ptr Agent::find_or_create_graph_manager auto it = graph_manager_map_.find(domain_id); if (it != graph_manager_map_.end()) { - return it->second(); + return it->second; }else{ return graph_manager_map_.insert( std::pair< @@ -217,7 +217,7 @@ std::shared_ptr Agent::find_or_create_graph_manager domain_id, std::make_shared(domain_id) ) - ); + ).first->second; } } From 8f01ee64fef6137c05441ee2f1ce491b18cc34f4 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 21 Jun 2021 16:11:16 +0200 Subject: [PATCH 5/5] Update micro_ros_agent/include/agent/graph_manager/graph_manager.hpp Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com> --- micro_ros_agent/include/agent/graph_manager/graph_manager.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 e141095..af4a7ac 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -78,7 +78,7 @@ class GraphManager GraphManager(eprosima::fastdds::dds::DomainId_t domain_id); /** - * @brief Default destrucctor. + * @brief Default destructor. */ ~GraphManager() = default; @@ -320,4 +320,4 @@ class GraphManager } // namespace agent } // namespace uros -#endif // _UROS_AGENT_GRAPH_MANAGER_HPP \ No newline at end of file +#endif // _UROS_AGENT_GRAPH_MANAGER_HPP