From a313a088bd46e25c09b46431c3eb9451c72139b7 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 2 Jul 2021 09:48:38 +0200 Subject: [PATCH 01/21] Fix graph manager node namespaces (#72) (#74) (cherry picked from commit a62330850541b80fdfb22fba40508eec7e1f3934) Co-authored-by: Pablo Garrido --- .../src/agent/graph_manager/graph_manager.cpp | 32 +++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) 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 ba61daa..e7904ef 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -309,8 +309,36 @@ void GraphManager::add_participant( if (node_name != enclave) // Do not add root node { + // Split node name in domain and node name + std::istringstream iss(node_name); + std::vector result; + std::string token; + + while(std::getline(iss, token, '/')) + { + result.push_back(token); + } + + std::string isolated_node_name = ""; + std::string isolated_namespace = ""; + + if (result.size() > 1) + { + isolated_namespace = result[0]; + for (size_t i = 1; i < result.size(); i++) + { + isolated_node_name.append(result[i] + "/"); + } + isolated_node_name.pop_back(); + } + else + { + isolated_node_name = node_name; + isolated_namespace = ""; + } + rmw_dds_common::msg::ParticipantEntitiesInfo info = - graphCache_.add_node(gid, node_name, enclave_); + graphCache_.add_node(gid, isolated_node_name, isolated_namespace); ros_discovery_datawriter_->write(static_cast(&info)); } } @@ -587,7 +615,7 @@ static eprosima::fastdds::dds::DataReaderQos reader_qos_conversion( const eprosima::fastdds::dds::ReaderQos& reader_qos) { eprosima::fastdds::dds::DataReaderQos datareader_qos; - + datareader_qos.durability(reader_qos.m_durability); datareader_qos.deadline(reader_qos.m_deadline); datareader_qos.latency_budget(reader_qos.m_latencyBudget); From ee955ed01e4d20a89c112ecbe3fac1baf2bef4b1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 2 Jul 2021 13:42:25 +0200 Subject: [PATCH 02/21] Fix graph manager node namespaces (#75) (#77) * Fix graph manager node namespaces * Update (cherry picked from commit f6a2aa3b6040978916cad116dcf3d72fecfc3109) Co-authored-by: Pablo Garrido --- .../agent/graph_manager/graph_manager.hpp | 12 +++ .../src/agent/graph_manager/graph_manager.cpp | 76 ++++++++++++------- 2 files changed, 59 insertions(+), 29 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 377bf82..1ef6e41 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -262,6 +262,18 @@ class GraphManager */ void update_node_entities_info(); + /** + * @brief Retrieves node name and namespace from + * participant. + * @param participant_name DDS participant name. + * @param node_name ROS2 Node name. + * @param namespace ROS2 Node namespace. + */ + void get_name_and_namespace( + std::string participant_name, + std::string& node_name, + std::string& node_namespace); + eprosima::fastdds::dds::DomainId_t domain_id_; bool graph_changed_; bool display_on_change_; 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 e7904ef..78d35bb 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -37,6 +37,8 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) , microros_graph_info_typesupport_(std::make_unique< eprosima::fastdds::dds::TypeSupport>(new graph_manager::MicrorosGraphInfoTypeSupport())) { + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles(); + // Create DomainParticipant eprosima::fastdds::dds::DomainParticipantQos participant_qos = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos(); @@ -309,33 +311,8 @@ void GraphManager::add_participant( if (node_name != enclave) // Do not add root node { - // Split node name in domain and node name - std::istringstream iss(node_name); - std::vector result; - std::string token; - - while(std::getline(iss, token, '/')) - { - result.push_back(token); - } - - std::string isolated_node_name = ""; - std::string isolated_namespace = ""; - - if (result.size() > 1) - { - isolated_namespace = result[0]; - for (size_t i = 1; i < result.size(); i++) - { - isolated_node_name.append(result[i] + "/"); - } - isolated_node_name.pop_back(); - } - else - { - isolated_node_name = node_name; - isolated_namespace = ""; - } + std::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); @@ -441,14 +418,18 @@ void GraphManager::associate_entity( { case dds::xrce::OBJK_DATAWRITER: { + std::string isolated_node_name, isolated_namespace; + get_name_and_namespace(qos.name().c_str(), isolated_node_name, isolated_namespace); info = graphCache_.associate_writer( - entity_gid, participant_gid, qos.name().c_str(), enclave_); + entity_gid, participant_gid, isolated_node_name, isolated_namespace); break; } case dds::xrce::OBJK_DATAREADER: { + std::string isolated_node_name, isolated_namespace; + get_name_and_namespace(qos.name().c_str(), isolated_node_name, isolated_namespace); info = graphCache_.associate_reader( - entity_gid, participant_gid, qos.name().c_str(), enclave_); + entity_gid, participant_gid, isolated_node_name, isolated_namespace); break; } default: @@ -548,6 +529,43 @@ void GraphManager::update_node_entities_info() } } +void GraphManager::get_name_and_namespace( + std::string participant_name, + std::string& node_name, + std::string& node_namespace) +{ + // Remove first / if exists + if (participant_name.rfind("/", 0) == 0) + { + participant_name.erase(participant_name.begin()); + } + + // Split node name in domain and node name + std::istringstream iss(participant_name); + std::vector result; + std::string token; + + while(std::getline(iss, token, '/')) + { + result.push_back(token); + } + + if (result.size() > 1) + { + node_namespace = "/" + result[0]; + for (size_t i = 1; i < result.size(); i++) + { + node_name.append(result[i] + "/"); + } + node_name.pop_back(); + } + else + { + node_name = participant_name; + node_namespace = "/"; + } +} + GraphManager::ParticipantListener::ParticipantListener( GraphManager* graph_manager) : eprosima::fastdds::dds::DomainParticipantListener() From a8caaa1b727a2d5a951090b22ac898ab882824c4 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 5 Jul 2021 10:56:22 +0200 Subject: [PATCH 03/21] Fix agent launch (#78) (#80) Co-authored-by: Your Name (cherry picked from commit 69f17c28a8d257893517ce1dfc50efd8972ef834) Co-authored-by: Pablo Garrido --- micro_ros_agent/launch/micro_ros_agent_launch.py | 2 +- micro_ros_agent/src/main.cpp | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/micro_ros_agent/launch/micro_ros_agent_launch.py b/micro_ros_agent/launch/micro_ros_agent_launch.py index d9071f0..4c564bd 100644 --- a/micro_ros_agent/launch/micro_ros_agent_launch.py +++ b/micro_ros_agent/launch/micro_ros_agent_launch.py @@ -7,6 +7,6 @@ def generate_launch_description(): package='micro_ros_agent', executable='micro_ros_agent', name='micro_ros_agent', - arguments={'udp4 --port 8888 -v6'} + arguments={"udp4", "-p", "8888", "-v6"} ) ]) diff --git a/micro_ros_agent/src/main.cpp b/micro_ros_agent/src/main.cpp index c9333ab..d5b29bf 100644 --- a/micro_ros_agent/src/main.cpp +++ b/micro_ros_agent/src/main.cpp @@ -27,6 +27,11 @@ int main(int argc, char** argv) std::vector params; for (int i = 0; i < argc; ++i) { + if(strcmp("--ros-args", argv[i]) == 0) + { + argc = i; + break; + } params.emplace_back(std::string(argv[i])); } From e43cde14ae836123391d168ef11391d16dc30d20 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 8 Jul 2021 15:33:15 +0200 Subject: [PATCH 04/21] Graph manager: Fix participant mask for listener callbacks (#81) (#82) (cherry picked from commit 95fecd77ae5fbd7cb007d06d1bc31b3387408b13) Co-authored-by: Pablo Garrido --- micro_ros_agent/src/agent/graph_manager/graph_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 78d35bb..c5106b0 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -54,8 +54,9 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) participant_qos.wire_protocol().builtin.writerHistoryMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + eprosima::fastdds::dds::StatusMask par_mask = eprosima::fastdds::dds::StatusMask::none(); 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(), par_mask)); // Register participant within typesupport participant_->register_type(*participant_info_typesupport_); From 256424b7ec2fcad0047efd9f697538b67de93579 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 9 Jul 2021 13:41:31 +0200 Subject: [PATCH 05/21] Fix graph manager datawriters behaviour (#84) (#85) * Fix graph manager datawriters Fixes Fix Fix destruction * Revert "Fix graph manager datawriters" This reverts commit 2847a2ebc3bdc0760837c3b86f2ff4f48e86592d. * Initial (cherry picked from commit 646ca52aab4e03620938562086896d05c2c950ee) Co-authored-by: Pablo Garrido --- .../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( From ff3b6f3b640e6e00fe26948223979a8acf163c8f Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Wed, 21 Jul 2021 13:22:20 +0200 Subject: [PATCH 06/21] Add stop functionality --- micro_ros_agent/include/agent/Agent.hpp | 2 ++ micro_ros_agent/src/agent/Agent.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index f5f22f3..6860249 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -38,6 +38,8 @@ class Agent void run(); + void stop(); + private: eprosima::uxr::AgentInstance& xrce_dds_agent_instance_; diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 87ef3cf..d8ae538 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -176,6 +176,11 @@ void Agent::run() return xrce_dds_agent_instance_.run(); } +void Agent::stop() +{ + return xrce_dds_agent_instance_.stop(); +} + std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) { From 4bcd9f23376677423c305be756e1f843377f246f Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Wed, 21 Jul 2021 15:26:52 +0200 Subject: [PATCH 07/21] Add library build to cmake --- micro_ros_agent/CMakeLists.txt | 145 ++++++++++++++++++++++++++------- 1 file changed, 117 insertions(+), 28 deletions(-) diff --git a/micro_ros_agent/CMakeLists.txt b/micro_ros_agent/CMakeLists.txt index d0b63c0..f970e38 100644 --- a/micro_ros_agent/CMakeLists.txt +++ b/micro_ros_agent/CMakeLists.txt @@ -14,6 +14,9 @@ cmake_minimum_required(VERSION 3.5) +option(BUILD_SHARED_LIBS "Control shared/static building." ON) +option(UBUILD_AGENT_EXECUTABLE "Control shared/static building." ON) + option(UROSAGENT_GENERATE_PROFILE "Generates agent.refs according to the .msgs provided in the .repos" OFF ) @@ -41,15 +44,26 @@ find_package(ament_cmake_gtest REQUIRED) find_package(micro_ros_msgs REQUIRED) -add_executable(${PROJECT_NAME} - src/main.cpp +include(GNUInstallDirs) +set(BIN_INSTALL_DIR ${CMAKE_INSTALL_BINDIR} CACHE PATH "Installation directory for binaries") +set(INCLUDE_INSTALL_DIR ${CMAKE_INSTALL_INCLUDEDIR} CACHE PATH "Installation directory for C headers") +set(LIB_INSTALL_DIR ${CMAKE_INSTALL_LIBDIR} CACHE PATH "Installation directory for libraries") +set(DATA_INSTALL_DIR ${CMAKE_INSTALL_DATADIR} CACHE PATH "Installation directory for data") + +# Set source files +set(SRCS src/agent/Agent.cpp src/agent/graph_manager/graph_manager.cpp src/agent/graph_manager/graph_typesupport.cpp src/agent/utils/demangle.cpp ) -target_include_directories(${PROJECT_NAME} +# Library +add_library(${PROJECT_NAME} ${SRCS}) + +target_include_directories(${PROJECT_NAME} BEFORE + PUBLIC + $ PRIVATE include ) @@ -76,16 +90,9 @@ target_link_libraries(${PROJECT_NAME} target_compile_options(${PROJECT_NAME} PRIVATE - $<$:-Wall> - $<$:-Wextra> - $<$:-pedantic> - ) - -set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD - 14 - CXX_STANDARD_REQUIRED - YES + $<$,$>:-Wall> + $<$,$>:-Wextra> + $<$,$>:-Wpedantic> ) set_target_properties(${PROJECT_NAME} PROPERTIES @@ -95,31 +102,113 @@ set_target_properties(${PROJECT_NAME} PROPERTIES YES ) -target_compile_options(${PROJECT_NAME} - PRIVATE - $<$,$>:-Wall> - $<$,$>:-Wextra> - $<$,$>:-Wpedantic> - ) - -ament_export_dependencies(microxrcedds_agent) +# Install agent lib +install( + TARGETS + ${PROJECT_NAME} + EXPORT + ${PROJECT_NAME}Targets + RUNTIME DESTINATION + ${BIN_INSTALL_DIR} + LIBRARY DESTINATION + ${LIB_INSTALL_DIR} + ARCHIVE DESTINATION + ${LIB_INSTALL_DIR} + COMPONENT + libraries + ) -ament_package() +# Install includes +install( + DIRECTORY + ${PROJECT_SOURCE_DIR}/include/agent + DESTINATION + ${INCLUDE_INSTALL_DIR} + FILES_MATCHING + PATTERN "*.hpp" + PATTERN "*.h" + ) +# Export library install( - TARGETS - ${PROJECT_NAME} + EXPORT + ${PROJECT_NAME}Targets DESTINATION - lib/${PROJECT_NAME} + ${DATA_INSTALL_DIR}/${PROJECT_NAME}/cmake ) +# Package configuration +include(CMakePackageConfigHelpers) + +configure_package_config_file( + ${PROJECT_SOURCE_DIR}/cmake/Config.cmake.in + ${PROJECT_BINARY_DIR}/cmake/config/${PROJECT_NAME}Config.cmake + INSTALL_DESTINATION + ${DATA_INSTALL_DIR}/${PROJECT_NAME}/cmake + PATH_VARS + BIN_INSTALL_DIR + INCLUDE_INSTALL_DIR + LIB_INSTALL_DIR + DATA_INSTALL_DIR + ) + install( - DIRECTORY - launch + FILES + ${PROJECT_BINARY_DIR}/cmake/config/${PROJECT_NAME}Config.cmake DESTINATION - share/${PROJECT_NAME} + ${DATA_INSTALL_DIR}/${PROJECT_NAME}/cmake ) +if(UBUILD_AGENT_EXECUTABLE) + add_executable(micro_ROS_agent + ${SRCS} + src/main.cpp + ) + + target_include_directories(micro_ROS_agent + PRIVATE + $ + ) + + ament_target_dependencies(micro_ROS_agent + rosidl_typesupport_fastrtps_cpp + rosidl_runtime_cpp + rosidl_typesupport_cpp + fastcdr + fastrtps + rmw_dds_common + rmw + rmw_fastrtps_shared_cpp + micro_ros_msgs + ) + + target_link_libraries(micro_ROS_agent + microxrcedds_agent + fastcdr + fastrtps + $<$>:rt> + $<$>:dl> + ) + + install( + TARGETS + micro_ROS_agent + DESTINATION + lib/${PROJECT_NAME} + ) + + install( + DIRECTORY + launch + DESTINATION + share/${PROJECT_NAME} + ) +endif() + +ament_export_dependencies(microxrcedds_agent) + +ament_package() + if(UROSAGENT_GENERATE_PROFILE) set(_OUTPUT_PATH "${CMAKE_CURRENT_BINARY_DIR}/python") From ce149628b4995c596f70b0fec5fa705404e0b06f Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Thu, 22 Jul 2021 07:46:36 +0200 Subject: [PATCH 08/21] Add cmake config file --- micro_ros_agent/cmake/Config.cmake.in | 32 +++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 micro_ros_agent/cmake/Config.cmake.in diff --git a/micro_ros_agent/cmake/Config.cmake.in b/micro_ros_agent/cmake/Config.cmake.in new file mode 100644 index 0000000..cecf44e --- /dev/null +++ b/micro_ros_agent/cmake/Config.cmake.in @@ -0,0 +1,32 @@ +# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set(@PROJECT_NAME@_VERSION @PROJECT_VERSION@) + +@PACKAGE_INIT@ + +if(MSVC OR MSVC_IDE) + set_and_check(@PROJECT_NAME@_BIN_DIR "@PACKAGE_BIN_INSTALL_DIR@") +endif() +set_and_check(@PROJECT_NAME@_INCLUDE_DIR "@PACKAGE_INCLUDE_INSTALL_DIR@") +set_and_check(@PROJECT_NAME@_LIB_DIR "@PACKAGE_LIB_INSTALL_DIR@") +set_and_check(@PROJECT_NAME@_DATA_DIR "@PACKAGE_DATA_INSTALL_DIR@") + +foreach(d @_deps@) + list(GET d 0 _name) + list(GET d 1 _version) + find_package(${_name} ${_version} REQUIRED) +endforeach() + +include(${@PROJECT_NAME@_DATA_DIR}/@PROJECT_NAME@/cmake/@PROJECT_NAME@Targets.cmake) From 43a1ce2283ff530827d1be6e12031662d8e205b9 Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Thu, 22 Jul 2021 07:57:54 +0200 Subject: [PATCH 09/21] Delete void returns --- micro_ros_agent/src/agent/Agent.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index d8ae538..9119838 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -173,12 +173,12 @@ bool Agent::create( void Agent::run() { - return xrce_dds_agent_instance_.run(); + xrce_dds_agent_instance_.run(); } void Agent::stop() { - return xrce_dds_agent_instance_.stop(); + xrce_dds_agent_instance_.stop(); } std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) From 790c937dc23a779ef5326492e8df3a3af8069c0d Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Thu, 22 Jul 2021 07:59:53 +0200 Subject: [PATCH 10/21] Change back binary name to micro_ros_agent --- micro_ros_agent/CMakeLists.txt | 29 ++++++++--------------------- 1 file changed, 8 insertions(+), 21 deletions(-) diff --git a/micro_ros_agent/CMakeLists.txt b/micro_ros_agent/CMakeLists.txt index f970e38..0dc866c 100644 --- a/micro_ros_agent/CMakeLists.txt +++ b/micro_ros_agent/CMakeLists.txt @@ -160,39 +160,26 @@ install( ) if(UBUILD_AGENT_EXECUTABLE) - add_executable(micro_ROS_agent + add_executable(micro_ros_agent_bin ${SRCS} src/main.cpp ) - target_include_directories(micro_ROS_agent + target_include_directories(micro_ros_agent_bin PRIVATE $ ) - ament_target_dependencies(micro_ROS_agent - rosidl_typesupport_fastrtps_cpp - rosidl_runtime_cpp - rosidl_typesupport_cpp - fastcdr - fastrtps - rmw_dds_common - rmw - rmw_fastrtps_shared_cpp - micro_ros_msgs - ) + ament_target_dependencies(micro_ros_agent_bin ${PROJECT_NAME}) - target_link_libraries(micro_ROS_agent - microxrcedds_agent - fastcdr - fastrtps - $<$>:rt> - $<$>:dl> - ) + target_link_libraries(micro_ros_agent_bin ${PROJECT_NAME}) + + set_target_properties(micro_ros_agent_bin + PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) install( TARGETS - micro_ROS_agent + micro_ros_agent_bin DESTINATION lib/${PROJECT_NAME} ) From 3eb34010f043087978ccdc00ce67b6a9d2fcaacd Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Thu, 22 Jul 2021 09:08:36 +0200 Subject: [PATCH 11/21] Delete unnecesary command --- micro_ros_agent/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/micro_ros_agent/CMakeLists.txt b/micro_ros_agent/CMakeLists.txt index 0dc866c..da6830a 100644 --- a/micro_ros_agent/CMakeLists.txt +++ b/micro_ros_agent/CMakeLists.txt @@ -170,8 +170,6 @@ if(UBUILD_AGENT_EXECUTABLE) $ ) - ament_target_dependencies(micro_ros_agent_bin ${PROJECT_NAME}) - target_link_libraries(micro_ros_agent_bin ${PROJECT_NAME}) set_target_properties(micro_ros_agent_bin From e9bf7fa65be69ecdd37409079bb90f16e61137db Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Thu, 22 Jul 2021 15:46:47 +0200 Subject: [PATCH 12/21] Update Graph manager destructor --- .../agent/graph_manager/graph_manager.hpp | 43 ++++++++++++++----- .../src/agent/graph_manager/graph_manager.cpp | 41 ++++++++++-------- 2 files changed, 56 insertions(+), 28 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 f0d94d2..b02c300 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -80,7 +80,30 @@ class GraphManager /** * @brief Default destructor. */ - ~GraphManager() = default; + ~GraphManager() + { + graphCache_.clear_on_change_callback(); + + exit = true; + cv_.notify_one(); + + if (microros_graph_publisher_.joinable()) + { + microros_graph_publisher_.join(); + } + + subscriber_->delete_datareader(ros_discovery_datareader_); + publisher_->delete_datawriter(ros_to_microros_graph_datawriter_); + + participant_->delete_subscriber(subscriber_); + participant_->delete_publisher(publisher_); + + // Delete topics + participant_->delete_topic(ros_discovery_topic_); + participant_->delete_topic(ros_to_microros_graph_topic_); + + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant(participant_); + } /** * @brief Implementation of the notification logic that updates the micro-ROS graph. @@ -274,6 +297,7 @@ class GraphManager std::thread microros_graph_publisher_; std::mutex mtx_; std::condition_variable cv_; + volatile bool exit = false; eprosima::fastdds::dds::DataWriterQos datawriter_qos_; @@ -283,18 +307,17 @@ class GraphManager std::unique_ptr participant_info_typesupport_; std::unique_ptr microros_graph_info_typesupport_; - std::unique_ptr participant_; - std::unique_ptr publisher_; - std::unique_ptr subscriber_; - std::unique_ptr ros_discovery_topic_; - std::unique_ptr ros_to_microros_graph_topic_; - std::unique_ptr ros_to_microros_graph_datawriter_; - std::unique_ptr ros_discovery_datareader_; - + eprosima::fastdds::dds::DomainParticipant* participant_; + eprosima::fastdds::dds::Publisher* publisher_; + eprosima::fastdds::dds::Subscriber* subscriber_; + eprosima::fastdds::dds::Topic* ros_discovery_topic_; + eprosima::fastdds::dds::Topic* ros_to_microros_graph_topic_; + eprosima::fastdds::dds::DataWriter* ros_to_microros_graph_datawriter_; + eprosima::fastdds::dds::DataReader* 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 + eprosima::fastdds::dds::DataWriter* > micro_ros_graph_datawriters_; }; 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 9badffc..8c5809a 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -54,29 +54,29 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; eprosima::fastdds::dds::StatusMask par_mask = eprosima::fastdds::dds::StatusMask::none(); - participant_.reset(eprosima::fastdds::dds::DomainParticipantFactory::get_instance()-> - create_participant(domain_id_, participant_qos, participant_listener_.get(), par_mask)); + participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()-> + create_participant(domain_id_, participant_qos, participant_listener_.get(), par_mask); // Register participant within typesupport participant_->register_type(*participant_info_typesupport_); participant_->register_type(*microros_graph_info_typesupport_); // Create publisher - publisher_.reset(participant_->create_publisher( - eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT)); + publisher_ = participant_->create_publisher( + eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT); // Create subscriber - subscriber_.reset(participant_->create_subscriber( - eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT)); + subscriber_ = participant_->create_subscriber( + eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT); // Create topics - ros_discovery_topic_.reset(participant_->create_topic("ros_discovery_info", + ros_discovery_topic_ = participant_->create_topic("ros_discovery_info", participant_info_typesupport_->get_type_name(), - eprosima::fastdds::dds::TOPIC_QOS_DEFAULT)); + eprosima::fastdds::dds::TOPIC_QOS_DEFAULT); - ros_to_microros_graph_topic_.reset(participant_->create_topic("ros_to_microros_graph", + ros_to_microros_graph_topic_ = participant_->create_topic("ros_to_microros_graph", microros_graph_info_typesupport_->get_type_name(), - eprosima::fastdds::dds::TOPIC_QOS_DEFAULT)); + eprosima::fastdds::dds::TOPIC_QOS_DEFAULT); // Create datawriters datawriter_qos_ = @@ -97,8 +97,8 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) 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(), ros_to_microros_datawriter_qos_)); + ros_to_microros_graph_datawriter_ = + publisher_->create_datawriter(ros_to_microros_graph_topic_, ros_to_microros_datawriter_qos_); // Create datareaders @@ -114,9 +114,9 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) datareader_qos.durability().kind = eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS; - ros_discovery_datareader_.reset( - subscriber_->create_datareader(ros_discovery_topic_.get(), - datareader_qos, datareader_listener_.get())); + ros_discovery_datareader_ = + subscriber_->create_datareader(ros_discovery_topic_, + datareader_qos, datareader_listener_.get()); // Set graph cache on change callback function graphCache_.set_on_change_callback([this]() @@ -137,10 +137,15 @@ inline void GraphManager::publish_microros_graph() std::unique_lock lock(mtx_); cv_.wait(lock, [this]() { - return this->graph_changed_; + return this->graph_changed_ || exit; }); } + if (exit) + { + break; + } + if (display_on_change_) { std::cout << "Updated uros Graph: graph changed" << std::endl; @@ -315,8 +320,8 @@ void GraphManager::add_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_)); + eprosima::fastdds::dds::DataWriter* datawriter; + datawriter = publisher_->create_datawriter(ros_discovery_topic_, datawriter_qos_); it = micro_ros_graph_datawriters_.insert( std::make_pair(participant, std::move(datawriter))).first; From 76c39a39be23ca51a11f85c1ddb0dfc60f9356ac Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Fri, 23 Jul 2021 07:54:25 +0200 Subject: [PATCH 13/21] Add aux micro-ros publishers delete --- micro_ros_agent/src/agent/graph_manager/graph_manager.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 8c5809a..b4c82c7 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -344,8 +344,10 @@ void GraphManager::remove_participant( rmw_dds_common::convert_gid_to_msg(&gid, &info.gid); auto it = micro_ros_graph_datawriters_.find(participant); it->second->write(static_cast(&info)); + + publisher_->delete_datawriter(it->second); + micro_ros_graph_datawriters_.erase(participant); } - micro_ros_graph_datawriters_.erase(participant); } void GraphManager::add_datawriter( From 7294ba66d715e48a9fa6e61d12c6389bddbfaa71 Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Mon, 26 Jul 2021 13:10:04 +0200 Subject: [PATCH 14/21] Update agent as singleton --- micro_ros_agent/include/agent/Agent.hpp | 16 +- micro_ros_agent/src/agent/Agent.cpp | 286 ++++++++++++------------ micro_ros_agent/src/main.cpp | 2 +- 3 files changed, 158 insertions(+), 146 deletions(-) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index 6860249..f27f4d2 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -26,11 +26,21 @@ namespace agent { class Agent { -public: - +private: Agent(); - ~Agent() = default; + Agent(const Agent &) = delete; + + Agent(Agent &&) = delete; + + Agent& operator =( + const Agent &) = delete; + + Agent& operator =( + Agent &&) = delete; + +public: + static Agent& getInstance(); bool create( int argc, diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 9119838..0dabe24 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -23,152 +23,154 @@ namespace agent { Agent::Agent() : xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance()) { + /** + * Add CREATE_PARTICIPANT callback. + */ + std::function on_create_participant + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant) -> void + { + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + graph_manager_->add_participant(participant); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT, + std::move(on_create_participant)); + + /** + * Add REMOVE_PARTICIPANT callback. + */ + std::function on_delete_participant + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant) -> void + { + 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, + eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT, + std::move(on_delete_participant)); + + /** + * Add CREATE_DATAWRITER callback. + */ + std::function on_create_datawriter + ([&]( + 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()); + + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datawriter->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + iHandle2GUID(instance_handle); + graph_manager_->add_datawriter(datawriter_guid, participant, datawriter); + graph_manager_->associate_entity( + datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER, + std::move(on_create_datawriter)); + + /** + * Add DELETE_DATAWRITER callback. + */ + std::function on_delete_datawriter + ([&]( + 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()); + + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datawriter->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); + graph_manager_->remove_datawriter(datawriter_guid); + }); + + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER, + std::move(on_delete_datawriter)); + + /** + * Add CREATE_DATAREADER callback. + */ + std::function on_create_datareader + ([&]( + 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()); + + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datareader->get_instance_handle(); + 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( + datareader_guid, participant, dds::xrce::OBJK_DATAREADER); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER, + std::move(on_create_datareader)); + + /** + * Add DELETE_DATAREADER callback. + */ + std::function on_delete_datareader + ([&]( + 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()); + + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); + graph_manager_->remove_datareader(datareader_guid); + }); + + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER, + std::move(on_delete_datareader)); +} + +Agent& Agent::getInstance() +{ + static Agent instance; + return instance; } bool Agent::create( int argc, char** argv) { - bool result = xrce_dds_agent_instance_.create(argc, argv); - if (result) - { - /** - * Add CREATE_PARTICIPANT callback. - */ - std::function on_create_participant - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void - { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - graph_manager_->add_participant(participant); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT, - std::move(on_create_participant)); - - /** - * Add REMOVE_PARTICIPANT callback. - */ - std::function on_delete_participant - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void - { - 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, - eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT, - std::move(on_delete_participant)); - - /** - * Add CREATE_DATAWRITER callback. - */ - std::function on_create_datawriter - ([&]( - 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()); - - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datawriter->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datawriter_guid = - iHandle2GUID(instance_handle); - graph_manager_->add_datawriter(datawriter_guid, participant, datawriter); - graph_manager_->associate_entity( - datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER, - std::move(on_create_datawriter)); - - /** - * Add DELETE_DATAWRITER callback. - */ - std::function on_delete_datawriter - ([&]( - 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()); - - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datawriter->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datawriter_guid = - eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); - graph_manager_->remove_datawriter(datawriter_guid); - }); - - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER, - std::move(on_delete_datawriter)); - - /** - * Add CREATE_DATAREADER callback. - */ - std::function on_create_datareader - ([&]( - 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()); - - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datareader->get_instance_handle(); - 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( - datareader_guid, participant, dds::xrce::OBJK_DATAREADER); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER, - std::move(on_create_datareader)); - - /** - * Add DELETE_DATAREADER callback. - */ - std::function on_delete_datareader - ([&]( - 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()); - - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datareader->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datareader_guid = - eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); - graph_manager_->remove_datareader(datareader_guid); - }); - - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER, - std::move(on_delete_datareader)); - } - - return result; + stop(); + return xrce_dds_agent_instance_.create(argc, argv); } void Agent::run() @@ -179,12 +181,12 @@ void Agent::run() void Agent::stop() { xrce_dds_agent_instance_.stop(); + graph_manager_map_.clear(); } 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; diff --git a/micro_ros_agent/src/main.cpp b/micro_ros_agent/src/main.cpp index d5b29bf..0c4937c 100644 --- a/micro_ros_agent/src/main.cpp +++ b/micro_ros_agent/src/main.cpp @@ -16,7 +16,7 @@ int main(int argc, char** argv) { - uros::agent::Agent micro_ros_agent; + uros::agent::Agent& micro_ros_agent = micro_ros_agent.getInstance(); /** Bypass '--ros-args' flag, as we use our own CLI parser. * As a workaround for launch files, arguments will be passed from From 9564b943eefab846e864cd2d00eac8fb773b9490 Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Mon, 26 Jul 2021 13:43:57 +0200 Subject: [PATCH 15/21] Modify callbacks init --- micro_ros_agent/include/agent/Agent.hpp | 2 +- micro_ros_agent/src/agent/Agent.cpp | 279 ++++++++++++------------ 2 files changed, 145 insertions(+), 136 deletions(-) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index f27f4d2..3f5c1c0 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -51,7 +51,7 @@ class Agent void stop(); private: - + bool initialized = false; eprosima::uxr::AgentInstance& xrce_dds_agent_instance_; std::map> graph_manager_map_; diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 0dabe24..f8ca3fe 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -23,140 +23,6 @@ namespace agent { Agent::Agent() : xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance()) { - /** - * Add CREATE_PARTICIPANT callback. - */ - std::function on_create_participant - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void - { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - graph_manager_->add_participant(participant); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT, - std::move(on_create_participant)); - - /** - * Add REMOVE_PARTICIPANT callback. - */ - std::function on_delete_participant - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void - { - 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, - eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT, - std::move(on_delete_participant)); - - /** - * Add CREATE_DATAWRITER callback. - */ - std::function on_create_datawriter - ([&]( - 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()); - - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datawriter->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datawriter_guid = - iHandle2GUID(instance_handle); - graph_manager_->add_datawriter(datawriter_guid, participant, datawriter); - graph_manager_->associate_entity( - datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER, - std::move(on_create_datawriter)); - - /** - * Add DELETE_DATAWRITER callback. - */ - std::function on_delete_datawriter - ([&]( - 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()); - - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datawriter->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datawriter_guid = - eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); - graph_manager_->remove_datawriter(datawriter_guid); - }); - - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER, - std::move(on_delete_datawriter)); - - /** - * Add CREATE_DATAREADER callback. - */ - std::function on_create_datareader - ([&]( - 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()); - - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datareader->get_instance_handle(); - 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( - datareader_guid, participant, dds::xrce::OBJK_DATAREADER); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER, - std::move(on_create_datareader)); - - /** - * Add DELETE_DATAREADER callback. - */ - std::function on_delete_datareader - ([&]( - 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()); - - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datareader->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datareader_guid = - eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); - graph_manager_->remove_datareader(datareader_guid); - }); - - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER, - std::move(on_delete_datareader)); } Agent& Agent::getInstance() @@ -170,7 +36,150 @@ bool Agent::create( char** argv) { stop(); - return xrce_dds_agent_instance_.create(argc, argv); + + bool result = xrce_dds_agent_instance_.create(argc, argv); + + if (result && !initialized) + { + initialized = true; + + /** + * Add CREATE_PARTICIPANT callback. + */ + std::function on_create_participant + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant) -> void + { + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + graph_manager_->add_participant(participant); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT, + std::move(on_create_participant)); + + /** + * Add REMOVE_PARTICIPANT callback. + */ + std::function on_delete_participant + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant) -> void + { + 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, + eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT, + std::move(on_delete_participant)); + + /** + * Add CREATE_DATAWRITER callback. + */ + std::function on_create_datawriter + ([&]( + 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()); + + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datawriter->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + iHandle2GUID(instance_handle); + graph_manager_->add_datawriter(datawriter_guid, participant, datawriter); + graph_manager_->associate_entity( + datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER, + std::move(on_create_datawriter)); + + /** + * Add DELETE_DATAWRITER callback. + */ + std::function on_delete_datawriter + ([&]( + 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()); + + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datawriter->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); + graph_manager_->remove_datawriter(datawriter_guid); + }); + + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER, + std::move(on_delete_datawriter)); + + /** + * Add CREATE_DATAREADER callback. + */ + std::function on_create_datareader + ([&]( + 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()); + + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datareader->get_instance_handle(); + 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( + datareader_guid, participant, dds::xrce::OBJK_DATAREADER); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER, + std::move(on_create_datareader)); + + /** + * Add DELETE_DATAREADER callback. + */ + std::function on_delete_datareader + ([&]( + 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()); + + // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); + graph_manager_->remove_datareader(datareader_guid); + }); + + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER, + std::move(on_delete_datareader)); + } + + return result; } void Agent::run() From f7fca51e2ed84beba0c41720626864cf5ea217fd Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Tue, 27 Jul 2021 10:31:07 +0200 Subject: [PATCH 16/21] Update graph manager --- .../agent/graph_manager/graph_manager.hpp | 31 ++++++------------- micro_ros_agent/src/agent/Agent.cpp | 19 +++++------- .../src/agent/graph_manager/graph_manager.cpp | 24 +++++++------- 3 files changed, 29 insertions(+), 45 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 b02c300..63fc5e5 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -80,29 +80,16 @@ class GraphManager /** * @brief Default destructor. */ - ~GraphManager() - { - graphCache_.clear_on_change_callback(); - - exit = true; - cv_.notify_one(); + ~GraphManager() = default; + void stop() + { if (microros_graph_publisher_.joinable()) { + exit = true; + cv_.notify_one(); microros_graph_publisher_.join(); } - - subscriber_->delete_datareader(ros_discovery_datareader_); - publisher_->delete_datawriter(ros_to_microros_graph_datawriter_); - - participant_->delete_subscriber(subscriber_); - participant_->delete_publisher(publisher_); - - // Delete topics - participant_->delete_topic(ros_discovery_topic_); - participant_->delete_topic(ros_to_microros_graph_topic_); - - eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant(participant_); } /** @@ -302,11 +289,11 @@ class GraphManager eprosima::fastdds::dds::DataWriterQos datawriter_qos_; rmw_dds_common::GraphCache graphCache_; - std::unique_ptr participant_listener_; - std::unique_ptr datareader_listener_; + ParticipantListener participant_listener_; + DatareaderListener datareader_listener_; - std::unique_ptr participant_info_typesupport_; - std::unique_ptr microros_graph_info_typesupport_; + eprosima::fastdds::dds::TypeSupport participant_info_typesupport_; + eprosima::fastdds::dds::TypeSupport microros_graph_info_typesupport_; eprosima::fastdds::dds::DomainParticipant* participant_; eprosima::fastdds::dds::Publisher* publisher_; eprosima::fastdds::dds::Subscriber* subscriber_; diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index f8ca3fe..355c797 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -35,8 +35,6 @@ bool Agent::create( int argc, char** argv) { - stop(); - bool result = xrce_dds_agent_instance_.create(argc, argv); if (result && !initialized) @@ -190,22 +188,21 @@ void Agent::run() void Agent::stop() { xrce_dds_agent_instance_.stop(); - graph_manager_map_.clear(); + + for (auto & element : graph_manager_map_) + { + element.second.stop(); + } } -std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) +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; + return &it->second; }else{ - return graph_manager_map_.insert( - std::make_pair( - domain_id, - std::make_shared(domain_id) - ) - ).first->second; + return &graph_manager_map_.emplace(domain_id, domain_id).first->second; } } 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 b4c82c7..e8c87c2 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -28,14 +28,14 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) , mtx_() , cv_() , graphCache_() - , participant_listener_(std::make_unique(this)) - , datareader_listener_(std::make_unique(this)) - , participant_info_typesupport_(std::make_unique< - eprosima::fastdds::dds::TypeSupport>(new graph_manager::ParticipantEntitiesInfoTypeSupport())) - , microros_graph_info_typesupport_(std::make_unique< - eprosima::fastdds::dds::TypeSupport>(new graph_manager::MicrorosGraphInfoTypeSupport())) + , participant_listener_(this) + , datareader_listener_(this) + , participant_info_typesupport_() + , microros_graph_info_typesupport_() { eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles(); + participant_info_typesupport_ = (eprosima::fastdds::dds::TypeSupport) new graph_manager::ParticipantEntitiesInfoTypeSupport(); + microros_graph_info_typesupport_ = (eprosima::fastdds::dds::TypeSupport) new graph_manager::MicrorosGraphInfoTypeSupport(); // Create DomainParticipant eprosima::fastdds::dds::DomainParticipantQos participant_qos = @@ -55,11 +55,11 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastdds::dds::StatusMask par_mask = eprosima::fastdds::dds::StatusMask::none(); participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()-> - create_participant(domain_id_, participant_qos, participant_listener_.get(), par_mask); + create_participant(domain_id_, participant_qos, &participant_listener_, par_mask); // Register participant within typesupport - participant_->register_type(*participant_info_typesupport_); - participant_->register_type(*microros_graph_info_typesupport_); + participant_->register_type(participant_info_typesupport_); + participant_->register_type(microros_graph_info_typesupport_); // Create publisher publisher_ = participant_->create_publisher( @@ -71,11 +71,11 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) // Create topics ros_discovery_topic_ = participant_->create_topic("ros_discovery_info", - participant_info_typesupport_->get_type_name(), + participant_info_typesupport_.get_type_name(), eprosima::fastdds::dds::TOPIC_QOS_DEFAULT); ros_to_microros_graph_topic_ = participant_->create_topic("ros_to_microros_graph", - microros_graph_info_typesupport_->get_type_name(), + microros_graph_info_typesupport_.get_type_name(), eprosima::fastdds::dds::TOPIC_QOS_DEFAULT); // Create datawriters @@ -116,7 +116,7 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) ros_discovery_datareader_ = subscriber_->create_datareader(ros_discovery_topic_, - datareader_qos, datareader_listener_.get()); + datareader_qos, &datareader_listener_); // Set graph cache on change callback function graphCache_.set_on_change_callback([this]() From 107dea729e78e4534e622e9f3e2e10cb9dcf42ae Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Tue, 27 Jul 2021 11:00:01 +0200 Subject: [PATCH 17/21] Add missing declaration --- micro_ros_agent/include/agent/Agent.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index 3f5c1c0..3035a38 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -53,9 +53,9 @@ class Agent private: bool initialized = false; eprosima::uxr::AgentInstance& xrce_dds_agent_instance_; - std::map> graph_manager_map_; + std::map graph_manager_map_; - std::shared_ptr find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id); + graph_manager::GraphManager* find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id); }; } // namespace agent From 7f9913a8ff664c61d56b9179be9c801a5768d4ab Mon Sep 17 00:00:00 2001 From: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com> Date: Tue, 27 Jul 2021 16:02:09 +0200 Subject: [PATCH 18/21] Update micro_ros_agent/include/agent/graph_manager/graph_manager.hpp --- micro_ros_agent/include/agent/graph_manager/graph_manager.hpp | 2 -- 1 file changed, 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 d031ffe..8ad99c6 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -288,8 +288,6 @@ class GraphManager eprosima::fastdds::dds::DataWriterQos datawriter_qos_; - eprosima::fastdds::dds::DataWriterQos datawriter_qos_; - rmw_dds_common::GraphCache graphCache_; ParticipantListener participant_listener_; From 1b606c7e924ca249e6b5eb0d686d5fce4125f79e Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Wed, 28 Jul 2021 07:51:36 +0200 Subject: [PATCH 19/21] Add clear to graph manager --- micro_ros_agent/src/agent/Agent.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 1efe944..da6af8b 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -193,6 +193,8 @@ void Agent::stop() { element.second.stop(); } + + graph_manager_map_.clear(); } graph_manager::GraphManager* Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) From a86d88312824833446d8ec1acfaf5f757d2c6520 Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Wed, 28 Jul 2021 10:01:55 +0200 Subject: [PATCH 20/21] Keep graph manager alive until destruction --- .../agent/graph_manager/graph_manager.hpp | 20 +++++++++---------- micro_ros_agent/src/agent/Agent.cpp | 10 +--------- 2 files changed, 10 insertions(+), 20 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 8ad99c6..c3618b0 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -80,17 +80,15 @@ class GraphManager /** * @brief Default destructor. */ - ~GraphManager() = default; - - void stop() - { - if (microros_graph_publisher_.joinable()) - { - exit = true; - cv_.notify_one(); - microros_graph_publisher_.join(); - } - } + ~GraphManager() + { + if (microros_graph_publisher_.joinable()) + { + exit = true; + cv_.notify_one(); + microros_graph_publisher_.join(); + } + } /** * @brief Implementation of the notification logic that updates the micro-ROS graph. diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index da6af8b..cfef9c2 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -188,19 +188,11 @@ void Agent::run() void Agent::stop() { xrce_dds_agent_instance_.stop(); - - for (auto & element : graph_manager_map_) - { - element.second.stop(); - } - - graph_manager_map_.clear(); } graph_manager::GraphManager* 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; From 2ee51a45f313d295ed67a50d736e400babdf3660 Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Wed, 28 Jul 2021 12:28:48 +0200 Subject: [PATCH 21/21] Update graph destructor --- micro_ros_agent/include/agent/Agent.hpp | 5 +++ .../agent/graph_manager/graph_manager.hpp | 32 +++++++++++++------ micro_ros_agent/src/agent/Agent.cpp | 7 ++++ 3 files changed, 35 insertions(+), 9 deletions(-) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index 3035a38..a388bdf 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -29,6 +29,11 @@ class Agent private: Agent(); + ~Agent() + { + stop(); + } + Agent(const Agent &) = delete; Agent(Agent &&) = delete; 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 c3618b0..d72388c 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -80,15 +80,29 @@ class GraphManager /** * @brief Default destructor. */ - ~GraphManager() - { - if (microros_graph_publisher_.joinable()) - { - exit = true; - cv_.notify_one(); - microros_graph_publisher_.join(); - } - } + ~GraphManager() = default; + + void stop() + { + if (microros_graph_publisher_.joinable()) + { + exit = true; + cv_.notify_one(); + microros_graph_publisher_.join(); + } + + subscriber_->delete_datareader(ros_discovery_datareader_); + publisher_->delete_datawriter(ros_to_microros_graph_datawriter_); + + participant_->delete_subscriber(subscriber_); + participant_->delete_publisher(publisher_); + + // Delete topics + participant_->delete_topic(ros_discovery_topic_); + participant_->delete_topic(ros_to_microros_graph_topic_); + + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant(participant_); + } /** * @brief Implementation of the notification logic that updates the micro-ROS graph. diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index cfef9c2..6cc87ce 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -188,6 +188,13 @@ void Agent::run() void Agent::stop() { xrce_dds_agent_instance_.stop(); + + for (auto & element : graph_manager_map_) + { + element.second.stop(); + } + + graph_manager_map_.clear(); } graph_manager::GraphManager* Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id)