Add multi domain graph manager (#69)

* Add multi domain graph manager

* Update

* Fix

* Fix

* Update micro_ros_agent/include/agent/graph_manager/graph_manager.hpp

Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com>

Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com>

micro_ros_agent/include/agent/Agent.hpp

@@ -41,7 +41,9 @@ public:
 private:
 
     eprosima::uxr::AgentInstance& xrce_dds_agent_instance_;
-    std::unique_ptr<graph_manager::GraphManager> graph_manager_;
+    std::map<eprosima::fastdds::dds::DomainId_t, std::shared_ptr<graph_manager::GraphManager>> graph_manager_map_;
+
+    std::shared_ptr<graph_manager::GraphManager> find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id);
 };
 
 }  // namespace agent

micro_ros_agent/include/agent/graph_manager/graph_manager.hpp

@@ -75,7 +75,7 @@ public:
     /**
      * @brief   Default constructor.
      */
-    GraphManager();
+    GraphManager(eprosima::fastdds::dds::DomainId_t domain_id);
 
     /**
      * @brief   Default destructor.
@@ -292,6 +292,7 @@ private:
      */
     void update_node_entities_info();
 
+    eprosima::fastdds::dds::DomainId_t domain_id_;
     bool graph_changed_;
     bool display_on_change_;
     const char * enclave_;

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)
 {
 }
 
@@ -33,8 +32,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 +40,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 +61,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 +84,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();
@@ -96,9 +111,16 @@ 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
             {
+
+                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 +144,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 +171,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 +202,25 @@ void Agent::run()
     return xrce_dds_agent_instance_.run();
 }
 
+std::shared_ptr<graph_manager::GraphManager> 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<graph_manager::GraphManager>
+            >(
+                domain_id,
+                std::make_shared<graph_manager::GraphManager>(domain_id)
+            )
+        ).first->second;
+    }
+}
+
 }  // namespace agent
 }  // namespace uros
 #endif  // _UROS_AGENT_AGENT_CPP

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_);