nicco/micro-ROS-Agent
1
0
Fork 0
mirror of https://github.com/micro-ROS/micro-ROS-Agent.git synced 2026-09-06 18:05:17 +02:00
Code Issues Packages Projects Releases Wiki Activity

Fix graph manager datawriters

Browse Source 
Fixes


Fix


Fix destruction
This commit is contained in:
Pablo Garrido
2021-07-09 13:13:33 +02:00
parent 95fecd77ae
commit 2847a2ebc3
3 changed files with 119 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
micro_ros_agent/include/agent/graph_manager/graph_manager.hpp
+20 -17
View File
@@ -89,28 +89,22 @@ public:
/** /**
* @brief Adds a DDS participant to the graph tree. * @brief Adds a DDS participant to the graph tree.
* @param participant Pointer to the participant to be added to the graph. * @param participant eprosima::fastdds::dds::DomainParticipant to be added.
*/ * @param from_microros if this participant has been added from micro-ROS.
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 enclave ROS 2 enclave. * @param enclave ROS 2 enclave.
*/ */
void add_participant( void add_participant(
const eprosima::fastrtps::rtps::GUID_t& guid, eprosima::fastdds::dds::DomainParticipant* participant,
const std::string& node_name, bool from_microros = true,
const std::string& enclave); const std::string& enclave = "/");
/** /**
* @brief Removes a DDS participant from the graph tree. * @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( void remove_participant(
const eprosima::fastrtps::rtps::GUID_t& guid); eprosima::fastdds::dds::DomainParticipant* participant,
bool from_microros = true);
/** /**
* @brief Adds a DDS datawriter to the graph tree. * @brief Adds a DDS datawriter to the graph tree.
@@ -186,7 +180,7 @@ public:
*/ */
void associate_entity( void associate_entity(
const eprosima::fastrtps::rtps::GUID_t& entity_guid, const eprosima::fastrtps::rtps::GUID_t& entity_guid,
const eprosima::fastdds::dds::DomainParticipant* participant, eprosima::fastdds::dds::DomainParticipant* participant,
const dds::xrce::ObjectKind& entity_kind); const dds::xrce::ObjectKind& entity_kind);
private: private:
@@ -277,11 +271,12 @@ private:
eprosima::fastdds::dds::DomainId_t domain_id_; eprosima::fastdds::dds::DomainId_t domain_id_;
bool graph_changed_; bool graph_changed_;
bool display_on_change_; bool display_on_change_;
const char * enclave_;
std::thread microros_graph_publisher_; std::thread microros_graph_publisher_;
std::mutex mtx_; std::mutex mtx_;
std::condition_variable cv_; std::condition_variable cv_;
eprosima::fastdds::dds::DataWriterQos datawriter_qos_;
rmw_dds_common::GraphCache graphCache_; rmw_dds_common::GraphCache graphCache_;
std::unique_ptr<ParticipantListener> participant_listener_; std::unique_ptr<ParticipantListener> participant_listener_;
std::unique_ptr<DatareaderListener> datareader_listener_; std::unique_ptr<DatareaderListener> datareader_listener_;
@@ -293,9 +288,17 @@ private:
std::unique_ptr<eprosima::fastdds::dds::Subscriber> subscriber_; std::unique_ptr<eprosima::fastdds::dds::Subscriber> subscriber_;
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_discovery_topic_; std::unique_ptr<eprosima::fastdds::dds::Topic> ros_discovery_topic_;
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_to_microros_graph_topic_; std::unique_ptr<eprosima::fastdds::dds::Topic> ros_to_microros_graph_topic_;
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_discovery_datawriter_;
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_to_microros_graph_datawriter_; std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_to_microros_graph_datawriter_;
std::unique_ptr<eprosima::fastdds::dds::DataReader> ros_discovery_datareader_; std::unique_ptr<eprosima::fastdds::dds::DataReader> ros_discovery_datareader_;
// Store a auxiliary publishers and datawriter for each participant created in micro-ROS
std::map<
eprosima::fastdds::dds::DomainParticipant*,
std::pair<
std::unique_ptr<eprosima::fastdds::dds::Publisher>,
std::unique_ptr<eprosima::fastdds::dds::DataWriter>
>
> micro_ros_graph_datawriters_;
}; };
} // namespace graph_manager } // namespace graph_manager
micro_ros_agent/src/agent/Agent.cpp
+38 -65
View File
@@ -32,20 +32,16 @@ bool Agent::create(
bool result = xrce_dds_agent_instance_.create(argc, argv); bool result = xrce_dds_agent_instance_.create(argc, argv);
if (result) if (result)
{ {
auto graph_manager_ = find_or_create_graph_manager(0);
/** /**
* Add CREATE_PARTICIPANT callback. * Add CREATE_PARTICIPANT callback.
*/ */
std::function<void ( std::function<void (
const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
([&]( ([&](
const eprosima::fastdds::dds::DomainParticipant* participant) -> void eprosima::fastdds::dds::DomainParticipant* participant) -> void
{ {
auto graph_manager_ = auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
participant->get_domain_id()
)
);
graph_manager_->add_participant(participant); graph_manager_->add_participant(participant);
}); });
xrce_dds_agent_instance_.add_middleware_callback( xrce_dds_agent_instance_.add_middleware_callback(
@@ -57,17 +53,12 @@ bool Agent::create(
* Add REMOVE_PARTICIPANT callback. * Add REMOVE_PARTICIPANT callback.
*/ */
std::function<void ( std::function<void (
const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
([&]( ([&](
const eprosima::fastdds::dds::DomainParticipant* participant) -> void eprosima::fastdds::dds::DomainParticipant* participant) -> void
{ {
auto graph_manager_ = auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t( graph_manager_->remove_participant(participant);
participant->get_domain_id()
)
);
graph_manager_->remove_participant(participant->guid());
}); });
xrce_dds_agent_instance_.add_middleware_callback( xrce_dds_agent_instance_.add_middleware_callback(
eprosima::uxr::Middleware::Kind::FASTDDS, eprosima::uxr::Middleware::Kind::FASTDDS,
@@ -78,22 +69,18 @@ bool Agent::create(
* Add CREATE_DATAWRITER callback. * Add CREATE_DATAWRITER callback.
*/ */
std::function<void ( std::function<void (
const eprosima::fastdds::dds::DomainParticipant *, eprosima::fastdds::dds::DomainParticipant *,
const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
([&]( ([&](
const eprosima::fastdds::dds::DomainParticipant* participant, eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataWriter* datawriter) -> void eprosima::fastdds::dds::DataWriter* datawriter) -> void
{ {
auto graph_manager_ = auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
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 // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
datawriter->get_instance_handle(); datawriter->get_instance_handle();
const eprosima::fastrtps::rtps::GUID_t datawriter_guid = eprosima::fastrtps::rtps::GUID_t datawriter_guid =
iHandle2GUID(instance_handle); iHandle2GUID(instance_handle);
graph_manager_->add_datawriter(datawriter_guid, participant, datawriter); graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
graph_manager_->associate_entity( graph_manager_->associate_entity(
@@ -108,23 +95,19 @@ bool Agent::create(
* Add DELETE_DATAWRITER callback. * Add DELETE_DATAWRITER callback.
*/ */
std::function<void ( std::function<void (
const eprosima::fastdds::dds::DomainParticipant *, eprosima::fastdds::dds::DomainParticipant *,
const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
([&]( ([&](
const eprosima::fastdds::dds::DomainParticipant* participant, eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataWriter* datawriter) -> void eprosima::fastdds::dds::DataWriter* datawriter) -> void
{ {
auto graph_manager_ = auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
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 // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
datawriter->get_instance_handle(); datawriter->get_instance_handle();
const eprosima::fastrtps::rtps::GUID_t datawriter_guid = eprosima::fastrtps::rtps::GUID_t datawriter_guid =
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
graph_manager_->remove_datawriter(datawriter_guid); graph_manager_->remove_datawriter(datawriter_guid);
}); });
@@ -138,22 +121,18 @@ bool Agent::create(
* Add CREATE_DATAREADER callback. * Add CREATE_DATAREADER callback.
*/ */
std::function<void ( std::function<void (
const eprosima::fastdds::dds::DomainParticipant *, eprosima::fastdds::dds::DomainParticipant *,
const eprosima::fastdds::dds::DataReader*)> on_create_datareader eprosima::fastdds::dds::DataReader*)> on_create_datareader
([&]( ([&](
const eprosima::fastdds::dds::DomainParticipant* participant, eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataReader* datareader) -> void eprosima::fastdds::dds::DataReader* datareader) -> void
{ {
auto graph_manager_ = auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
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 // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
datareader->get_instance_handle(); datareader->get_instance_handle();
const eprosima::fastrtps::rtps::GUID_t datareader_guid = eprosima::fastrtps::rtps::GUID_t datareader_guid =
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
graph_manager_->add_datareader(datareader_guid, participant, datareader); graph_manager_->add_datareader(datareader_guid, participant, datareader);
graph_manager_->associate_entity( graph_manager_->associate_entity(
@@ -168,22 +147,18 @@ bool Agent::create(
* Add DELETE_DATAREADER callback. * Add DELETE_DATAREADER callback.
*/ */
std::function<void ( std::function<void (
const eprosima::fastdds::dds::DomainParticipant *, eprosima::fastdds::dds::DomainParticipant *,
const eprosima::fastdds::dds::DataReader *)> on_delete_datareader eprosima::fastdds::dds::DataReader *)> on_delete_datareader
([&]( ([&](
const eprosima::fastdds::dds::DomainParticipant* participant, eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataReader* datareader) -> void eprosima::fastdds::dds::DataReader* datareader) -> void
{ {
auto graph_manager_ = auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
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 // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
datareader->get_instance_handle(); datareader->get_instance_handle();
const eprosima::fastrtps::rtps::GUID_t datareader_guid = eprosima::fastrtps::rtps::GUID_t datareader_guid =
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
graph_manager_->remove_datareader(datareader_guid); graph_manager_->remove_datareader(datareader_guid);
}); });
@@ -204,16 +179,14 @@ void Agent::run()
std::shared_ptr<graph_manager::GraphManager> Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) std::shared_ptr<graph_manager::GraphManager> Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id)
{ {
auto it = graph_manager_map_.find(domain_id); auto it = graph_manager_map_.find(domain_id);
if (it != graph_manager_map_.end()) { if (it != graph_manager_map_.end()) {
return it->second; return it->second;
}else{ }else{
return graph_manager_map_.insert( return graph_manager_map_.insert(
std::pair< std::make_pair(
eprosima::fastdds::dds::DomainId_t,
std::shared_ptr<graph_manager::GraphManager>
>(
domain_id, domain_id,
std::make_shared<graph_manager::GraphManager>(domain_id) std::make_shared<graph_manager::GraphManager>(domain_id)
) )
micro_ros_agent/src/agent/graph_manager/graph_manager.cpp
+60 -38
View File
@@ -22,11 +22,9 @@ namespace agent {
namespace graph_manager { namespace graph_manager {
GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
// : eprosima::fastrtps::ParticipantListener()
: domain_id_(domain_id) : domain_id_(domain_id)
, graph_changed_(false) , graph_changed_(false)
, display_on_change_(false) , display_on_change_(false)
, enclave_("/")
, mtx_() , mtx_()
, cv_() , cv_()
, graphCache_() , graphCache_()
@@ -43,12 +41,13 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
eprosima::fastdds::dds::DomainParticipantQos participant_qos = eprosima::fastdds::dds::DomainParticipantQos participant_qos =
eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_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); participant_qos.user_data().resize(length);
snprintf(reinterpret_cast<char *>(participant_qos.user_data().data_vec().data()), snprintf(reinterpret_cast<char *>(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 = participant_qos.wire_protocol().builtin.readerHistoryMemoryPolicy =
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
participant_qos.wire_protocol().builtin.writerHistoryMemoryPolicy = 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)); eprosima::fastdds::dds::TOPIC_QOS_DEFAULT));
// Create datawriters // Create datawriters
eprosima::fastdds::dds::DataWriterQos datawriter_qos = datawriter_qos_ =
eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT; eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT;
datawriter_qos.history().kind = datawriter_qos_.history().kind =
eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS; eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
datawriter_qos.history().depth = 1; datawriter_qos_.history().depth = 1;
datawriter_qos.endpoint().history_memory_policy = datawriter_qos_.endpoint().history_memory_policy =
eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; 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; eprosima::fastdds::dds::PublishModeQosPolicyKind::ASYNCHRONOUS_PUBLISH_MODE;
datawriter_qos.reliability().kind = datawriter_qos_.reliability().kind =
eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS; eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
datawriter_qos.durability().kind = datawriter_qos_.durability().kind =
eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS; eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
ros_discovery_datawriter_.reset( eprosima::fastdds::dds::DataWriterQos ros_to_microros_datawriter_qos_ = datawriter_qos_;
publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos)); ros_to_microros_datawriter_qos_.history().kind =
datawriter_qos.history().kind =
eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS; eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS;
ros_to_microros_graph_datawriter_.reset( 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 // Create datareaders
@@ -295,37 +292,62 @@ inline void GraphManager::publish_microros_graph()
} }
void GraphManager::add_participant( void GraphManager::add_participant(
const eprosima::fastdds::dds::DomainParticipant* participant) eprosima::fastdds::dds::DomainParticipant* participant,
{ bool from_microros,
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 std::string& enclave) 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); 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; 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 = rmw_dds_common::msg::ParticipantEntitiesInfo info =
graphCache_.add_node(gid, isolated_node_name, isolated_namespace); graphCache_.add_node(gid, isolated_node_name, isolated_namespace);
ros_discovery_datawriter_->write(static_cast<void *>(&info));
auto it = micro_ros_graph_datawriters_.find(participant);
if (it == micro_ros_graph_datawriters_.end())
{
// Register participant within typesupport
participant->register_type(*participant_info_typesupport_);
// Create publisher
std::unique_ptr<eprosima::fastdds::dds::Publisher> publisher;
publisher.reset(participant->create_publisher(eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT));
// Create datawriter
std::unique_ptr<eprosima::fastdds::dds::DataWriter> datawriter;
datawriter.reset(publisher->create_datawriter(ros_discovery_topic_.get(), datawriter_qos_));
it = micro_ros_graph_datawriters_.insert(
std::make_pair(participant, std::make_pair(std::move(publisher), std::move(datawriter)))).first;
}
it->second.second->write(static_cast<void *>(&info));
} }
} }
void GraphManager::remove_participant( void GraphManager::remove_participant(
const eprosima::fastrtps::rtps::GUID_t& guid) 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); 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.second->write(static_cast<void *>(&info));
}
micro_ros_graph_datawriters_.erase(participant);
} }
void GraphManager::add_datawriter( void GraphManager::add_datawriter(
@@ -404,7 +426,7 @@ void GraphManager::remove_datareader(
void GraphManager::associate_entity( void GraphManager::associate_entity(
const eprosima::fastrtps::rtps::GUID_t& entity_guid, const eprosima::fastrtps::rtps::GUID_t& entity_guid,
const eprosima::fastdds::dds::DomainParticipant* participant, eprosima::fastdds::dds::DomainParticipant* participant,
const dds::xrce::ObjectKind& entity_kind) const dds::xrce::ObjectKind& entity_kind)
{ {
const rmw_gid_t entity_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( const rmw_gid_t entity_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
@@ -438,7 +460,9 @@ void GraphManager::associate_entity(
break; break;
} }
} }
ros_discovery_datawriter_->write(static_cast<void *>(&info));
auto it = micro_ros_graph_datawriters_.find(participant);
it->second.second->write(static_cast<void *>(&info));
} }
@@ -592,13 +616,13 @@ void GraphManager::ParticipantListener::on_participant_discovery(
const std::string enclave = const std::string enclave =
std::string(name_found->second.begin(), name_found->second.end()); 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; break;
} }
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT:
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT:
{ {
graphManager_from_->remove_participant(info.info.m_guid); graphManager_from_->remove_participant(participant, false);
break; break;
} }
default: default:
@@ -713,7 +737,6 @@ void GraphManager::ParticipantListener::on_subscriber_discovery(
eprosima::fastrtps::rtps::ReaderDiscoveryInfo&& info) eprosima::fastrtps::rtps::ReaderDiscoveryInfo&& info)
{ {
process_discovery_info<eprosima::fastrtps::rtps::ReaderDiscoveryInfo>(info); process_discovery_info<eprosima::fastrtps::rtps::ReaderDiscoveryInfo>(info);
// graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAREADER);
} }
void GraphManager::ParticipantListener::on_publisher_discovery( void GraphManager::ParticipantListener::on_publisher_discovery(
@@ -721,7 +744,6 @@ void GraphManager::ParticipantListener::on_publisher_discovery(
eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info) eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info)
{ {
process_discovery_info<eprosima::fastrtps::rtps::WriterDiscoveryInfo>(info); process_discovery_info<eprosima::fastrtps::rtps::WriterDiscoveryInfo>(info);
// graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAWRITER);
} }
GraphManager::DatareaderListener::DatareaderListener( GraphManager::DatareaderListener::DatareaderListener(