From 5c248aea281b8292d8361327b76f2910d3071c32 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 30 May 2022 12:10:19 +0200 Subject: [PATCH] Initial PoC Signed-off-by: Pablo Garrido --- micro_ros_agent/include/agent/Agent.hpp | 1 + .../agent/graph_manager/graph_manager.hpp | 3 ++- micro_ros_agent/src/agent/Agent.cpp | 16 +++++++++++++--- .../src/agent/graph_manager/graph_manager.cpp | 16 +++++++++++++--- 4 files changed, 29 insertions(+), 7 deletions(-) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index 92e4153..8383240 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -46,6 +46,7 @@ class Agent eprosima::uxr::AgentInstance& xrce_dds_agent_instance_; std::map> graph_manager_map_; + std::string namespace_remapping = ""; std::shared_ptr find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id); }; 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 4365909..d4f2e5e 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,7 @@ class GraphManager /** * @brief Default constructor. */ - GraphManager(eprosima::fastdds::dds::DomainId_t domain_id); + GraphManager(eprosima::fastdds::dds::DomainId_t domain_id, std::string namespace_remapping); /** * @brief Default destructor. @@ -274,6 +274,7 @@ class GraphManager std::string& node_namespace); eprosima::fastdds::dds::DomainId_t domain_id_; + std::string namespace_remapping_; bool graph_changed_; bool display_on_change_; std::thread microros_graph_publisher_; diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 12995d3..49e236f 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -33,6 +33,17 @@ bool Agent::create( char** argv) { bool result = xrce_dds_agent_instance_.create(argc, argv); + + // Find namespace remapping + for (int i = 0; i < argc; i++) + { + if (strcmp(argv[i], "--namespace-remapping") == 0 && i + 1 < argc) + { + namespace_remapping = argv[i + 1]; + std::cout << "Remapping all entities to namespace " << namespace_remapping << std::endl; + } + } + if (result) { /** @@ -293,8 +304,7 @@ 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; @@ -302,7 +312,7 @@ auto it = graph_manager_map_.find(domain_id); return graph_manager_map_.insert( std::make_pair( domain_id, - std::make_shared(domain_id) + std::make_shared(domain_id, namespace_remapping) ) ).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 2a85a91..713cbd1 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -26,8 +26,9 @@ namespace uros { namespace agent { namespace graph_manager { -GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) +GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id, std::string namespace_remapping) : domain_id_(domain_id) + , namespace_remapping_(namespace_remapping) , graph_changed_(false) , display_on_change_(false) , mtx_() @@ -313,6 +314,8 @@ void GraphManager::add_participant( std::string isolated_node_name, isolated_namespace; get_name_and_namespace(qos.name().to_string(), isolated_node_name, isolated_namespace); + isolated_namespace = namespace_remapping_ + isolated_namespace; + rmw_dds_common::msg::ParticipantEntitiesInfo info = graphCache_.add_node(gid, isolated_node_name, isolated_namespace); @@ -355,7 +358,10 @@ void GraphManager::add_datawriter( { const std::string& topic_name = datawriter->get_topic()->get_name(); const std::string& type_name = datawriter->get_topic()->get_type_name(); - this->add_datawriter(datawriter_guid, topic_name, type_name, + + auto remapped_topic_name = namespace_remapping_ + "/" + topic_name; + + this->add_datawriter(datawriter_guid, remapped_topic_name, type_name, participant->guid(), datawriter->get_qos()); } @@ -372,7 +378,9 @@ void GraphManager::add_datawriter( "rmw_fastrtps_cpp", participant_guid); const rmw_qos_profile_t qos_profile = fastdds_qos_to_rmw_qos(writer_qos); - graphCache_.add_entity(datawriter_gid, topic_name, + auto remapped_topic_name = namespace_remapping_ + "/" + topic_name; + + graphCache_.add_entity(datawriter_gid, remapped_topic_name, type_name, participant_gid, qos_profile, false); } @@ -441,6 +449,7 @@ void GraphManager::associate_entity( { std::string isolated_node_name, isolated_namespace; get_name_and_namespace(qos.name().c_str(), isolated_node_name, isolated_namespace); + isolated_namespace = namespace_remapping_ + isolated_namespace; info = graphCache_.associate_writer( entity_gid, participant_gid, isolated_node_name, isolated_namespace); break; @@ -449,6 +458,7 @@ void GraphManager::associate_entity( { std::string isolated_node_name, isolated_namespace; get_name_and_namespace(qos.name().c_str(), isolated_node_name, isolated_namespace); + isolated_namespace = namespace_remapping_ + isolated_namespace; info = graphCache_.associate_reader( entity_gid, participant_gid, isolated_node_name, isolated_namespace); break;