From 5635a054edd97fc08af110ad02f7257ea19a174a Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Fri, 2 Jul 2021 11:01:01 +0200 Subject: [PATCH 1/2] Fix graph manager node namespaces --- .../agent/graph_manager/graph_manager.hpp | 12 ++++ .../src/agent/graph_manager/graph_manager.cpp | 71 +++++++++++-------- 2 files changed, 54 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..5d8b951 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -309,33 +309,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 +416,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 +527,40 @@ void GraphManager::update_node_entities_info() } } +void GraphManager::get_name_and_namespace( + std::string participant_name, + std::string& node_name, + std::string& node_namespace) +{ + // 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); + } + + node_name = ""; + node_namespace = ""; + + 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 beaefa0fed5d8221bad27ca8bc97a9f6d5b9a509 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Fri, 2 Jul 2021 13:13:23 +0200 Subject: [PATCH 2/2] Update --- .../src/agent/graph_manager/graph_manager.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 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 5d8b951..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(); @@ -532,6 +534,12 @@ void GraphManager::get_name_and_namespace( 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; @@ -542,12 +550,9 @@ void GraphManager::get_name_and_namespace( result.push_back(token); } - node_name = ""; - node_namespace = ""; - if (result.size() > 1) { - node_namespace = result[0]; + node_namespace = "/" + result[0]; for (size_t i = 1; i < result.size(); i++) { node_name.append(result[i] + "/"); @@ -557,7 +562,7 @@ void GraphManager::get_name_and_namespace( else { node_name = participant_name; - node_namespace = ""; + node_namespace = "/"; } }