diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index ca351b5..f5f22f3 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 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..af4a7ac 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -75,7 +75,7 @@ class GraphManager /** * @brief Default constructor. */ - GraphManager(); + GraphManager(eprosima::fastdds::dds::DomainId_t domain_id); /** * @brief Default destructor. @@ -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_; @@ -319,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 diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 79f9d9e..72c4508 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) { } @@ -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 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) + ) + ).first->second; + } +} + } // 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_);