FazBrowse GitHub Viewer | Trending |
URL:
| Home
Tools: [Download Repo ZIP]   [Original HTTPS Page]

Fix graph manager datawriters behaviour (#84) · micro-ROS/micro-ROS-Agent@646ca52 · GitHub

/ micro-ROS-Agent Public

Commit 646ca52

Browse files
pablogs9
authored
Fix graph manager datawriters behaviour (#84)
* Fix graph manager datawriters Fixes Fix Fix destruction * Revert "Fix graph manager datawriters" This reverts commit 2847a2e. * Initial
1 parent 95fecd7 commit 646ca52

3 files changed

Lines changed: 78 additions & 91 deletions

File tree

‎micro_ros_agent/include/agent/graph_manager/graph_manager.hpp‎

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -89,28 +89,22 @@ class GraphManager
8989

9090
/**
9191
* @brief Adds a DDS participant to the graph tree.
92-
* @param participant Pointer to the participant to be added to the graph.
93-
*/
94-
void add_participant(
95-
const eprosima::fastdds::dds::DomainParticipant* participant);
96-
97-
/**
98-
* @brief Adds a DDS participant to the graph tree.
99-
* @param guid rtps::GUID_t of the participant to be added.
100-
* @param node_name Name of the ROS 2 node associated to the given participant.
92+
* @param participant eprosima::fastdds::dds::DomainParticipant to be added.
93+
* @param from_microros if this participant has been added from micro-ROS.
10194
* @param enclave ROS 2 enclave.
10295
*/
10396
void add_participant(
104-
const eprosima::fastrtps::rtps::GUID_t& guid,
105-
const std::string& node_name,
106-
const std::string& enclave);
97+
const eprosima::fastdds::dds::DomainParticipant* participant,
98+
bool from_microros = true,
99+
const std::string& enclave = "/");
107100

108101
/**
109102
* @brief Removes a DDS participant from the graph tree.
110-
* @param guid rtps::GUID_t of the participant to be removed.
103+
* @param participant eprosima::fastdds::dds::DomainParticipant to be removed.
111104
*/
112105
void remove_participant(
113-
const eprosima::fastrtps::rtps::GUID_t& guid);
106+
const eprosima::fastdds::dds::DomainParticipant* participant,
107+
bool from_microros = true);
114108

115109
/**
116110
* @brief Adds a DDS datawriter to the graph tree.
@@ -277,11 +271,12 @@ class GraphManager
277271
eprosima::fastdds::dds::DomainId_t domain_id_;
278272
bool graph_changed_;
279273
bool display_on_change_;
280-
const char * enclave_;
281274
std::thread microros_graph_publisher_;
282275
std::mutex mtx_;
283276
std::condition_variable cv_;
284277

278+
eprosima::fastdds::dds::DataWriterQos datawriter_qos_;
279+
285280
rmw_dds_common::GraphCache graphCache_;
286281
std::unique_ptr<ParticipantListener> participant_listener_;
287282
std::unique_ptr<DatareaderListener> datareader_listener_;
@@ -293,9 +288,14 @@ class GraphManager
293288
std::unique_ptr<eprosima::fastdds::dds::Subscriber> subscriber_;
294289
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_discovery_topic_;
295290
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_to_microros_graph_topic_;
296-
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_discovery_datawriter_;
297291
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_to_microros_graph_datawriter_;
298292
std::unique_ptr<eprosima::fastdds::dds::DataReader> ros_discovery_datareader_;
293+
294+
// Store a auxiliary publishers and datawriter for each participant created in micro-ROS
295+
std::map<
296+
const eprosima::fastdds::dds::DomainParticipant*,
297+
std::unique_ptr<eprosima::fastdds::dds::DataWriter>
298+
> micro_ros_graph_datawriters_;
299299
};
300300

301301
} // namespace graph_manager

‎micro_ros_agent/src/agent/Agent.cpp‎

Lines changed: 10 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -40,12 +40,7 @@ bool Agent::create(
4040
([&](
4141
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
4242
{
43-
auto graph_manager_ =
44-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
45-
participant->get_domain_id()
46-
)
47-
);
48-
43+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
4944
graph_manager_->add_participant(participant);
5045
});
5146
xrce_dds_agent_instance_.add_middleware_callback(
@@ -61,13 +56,8 @@ bool Agent::create(
6156
([&](
6257
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
6358
{
64-
auto graph_manager_ =
65-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
66-
participant->get_domain_id()
67-
)
68-
);
69-
70-
graph_manager_->remove_participant(participant->guid());
59+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
60+
graph_manager_->remove_participant(participant);
7161
});
7262
xrce_dds_agent_instance_.add_middleware_callback(
7363
eprosima::uxr::Middleware::Kind::FASTDDS,
@@ -84,11 +74,7 @@ bool Agent::create(
8474
const eprosima::fastdds::dds::DomainParticipant* participant,
8575
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
8676
{
87-
auto graph_manager_ =
88-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
89-
participant->get_domain_id()
90-
)
91-
);
77+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
9278

9379
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
9480
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
@@ -115,11 +101,7 @@ bool Agent::create(
115101
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
116102
{
117103

118-
auto graph_manager_ =
119-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
120-
participant->get_domain_id()
121-
)
122-
);
104+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
123105

124106
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
125107
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
@@ -144,11 +126,7 @@ bool Agent::create(
144126
const eprosima::fastdds::dds::DomainParticipant* participant,
145127
const eprosima::fastdds::dds::DataReader* datareader) -> void
146128
{
147-
auto graph_manager_ =
148-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
149-
participant->get_domain_id()
150-
)
151-
);
129+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
152130

153131
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
154132
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
@@ -174,11 +152,7 @@ bool Agent::create(
174152
const eprosima::fastdds::dds::DomainParticipant* participant,
175153
const eprosima::fastdds::dds::DataReader* datareader) -> void
176154
{
177-
auto graph_manager_ =
178-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
179-
participant->get_domain_id()
180-
)
181-
);
155+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
182156

183157
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
184158
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
@@ -204,16 +178,14 @@ void Agent::run()
204178

205179
std::shared_ptr<graph_manager::GraphManager> Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id)
206180
{
207-
auto it = graph_manager_map_.find(domain_id);
181+
182+
auto it = graph_manager_map_.find(domain_id);
208183

209184
if (it != graph_manager_map_.end()) {
210185
return it->second;
211186
}else{
212187
return graph_manager_map_.insert(
213-
std::pair<
214-
eprosima::fastdds::dds::DomainId_t,
215-
std::shared_ptr<graph_manager::GraphManager>
216-
>(
188+
std::make_pair(
217189
domain_id,
218190
std::make_shared<graph_manager::GraphManager>(domain_id)
219191
)

‎micro_ros_agent/src/agent/graph_manager/graph_manager.cpp‎

Lines changed: 52 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -22,11 +22,9 @@ namespace agent {
2222
namespace graph_manager {
2323

2424
GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
25-
// : eprosima::fastrtps::ParticipantListener()
2625
: domain_id_(domain_id)
2726
, graph_changed_(false)
2827
, display_on_change_(false)
29-
, enclave_("/")
3028
, mtx_()
3129
, cv_()
3230
, graphCache_()
@@ -43,12 +41,13 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
4341
eprosima::fastdds::dds::DomainParticipantQos participant_qos =
4442
eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos();
4543

46-
size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave_) + 1;
44+
const char * enclave = "/";
45+
size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1;
4746
participant_qos.user_data().resize(length);
4847
snprintf(reinterpret_cast<char *>(participant_qos.user_data().data_vec().data()),
49-
length, "enclave=%s;", enclave_);
48+
length, "enclave=%s;", enclave);
5049

51-
participant_qos.name(enclave_);
50+
participant_qos.name(enclave);
5251
participant_qos.wire_protocol().builtin.readerHistoryMemoryPolicy =
5352
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
5453
participant_qos.wire_protocol().builtin.writerHistoryMemoryPolicy =
@@ -80,28 +79,26 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
8079
eprosima::fastdds::dds::TOPIC_QOS_DEFAULT));
8180

8281
// Create datawriters
83-
eprosima::fastdds::dds::DataWriterQos datawriter_qos =
82+
datawriter_qos_ =
8483
eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT;
8584

86-
datawriter_qos.history().kind =
85+
datawriter_qos_.history().kind =
8786
eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
88-
datawriter_qos.history().depth = 1;
89-
datawriter_qos.endpoint().history_memory_policy =
87+
datawriter_qos_.history().depth = 1;
88+
datawriter_qos_.endpoint().history_memory_policy =
9089
eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
91-
datawriter_qos.publish_mode().kind =
90+
datawriter_qos_.publish_mode().kind =
9291
eprosima::fastdds::dds::PublishModeQosPolicyKind::ASYNCHRONOUS_PUBLISH_MODE;
93-
datawriter_qos.reliability().kind =
92+
datawriter_qos_.reliability().kind =
9493
eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
95-
datawriter_qos.durability().kind =
94+
datawriter_qos_.durability().kind =
9695
eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
9796

98-
ros_discovery_datawriter_.reset(
99-
publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos));
100-
101-
datawriter_qos.history().kind =
97+
eprosima::fastdds::dds::DataWriterQos ros_to_microros_datawriter_qos_ = datawriter_qos_;
98+
ros_to_microros_datawriter_qos_.history().kind =
10299
eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS;
103100
ros_to_microros_graph_datawriter_.reset(
104-
publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), datawriter_qos));
101+
publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), ros_to_microros_datawriter_qos_));
105102

106103
// Create datareaders
107104

@@ -295,37 +292,55 @@ inline void GraphManager::publish_microros_graph()
295292
}
296293

297294
void GraphManager::add_participant(
298-
const eprosima::fastdds::dds::DomainParticipant* participant)
299-
{
300-
const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos();
301-
this->add_participant(participant->guid(), qos.name().to_string(), enclave_);
302-
}
303-
304-
void GraphManager::add_participant(
305-
const eprosima::fastrtps::rtps::GUID_t& guid,
306-
const std::string& node_name,
295+
const eprosima::fastdds::dds::DomainParticipant* participant,
296+
bool from_microros,
307297
const std::string& enclave)
308298
{
309-
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid);
299+
const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos();
300+
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid());
310301

311302
graphCache_.add_participant(gid, enclave);
312303

313-
if (node_name != enclave) // Do not add root node
304+
// Do not add root node and
305+
// do not announce non-micro-ROS participants
306+
if (qos.name().to_string() != "/" && from_microros)
314307
{
315308
std::string isolated_node_name, isolated_namespace;
316-
get_name_and_namespace(node_name, isolated_node_name, isolated_namespace);
309+
get_name_and_namespace(qos.name().to_string(), isolated_node_name, isolated_namespace);
317310

318311
rmw_dds_common::msg::ParticipantEntitiesInfo info =
319312
graphCache_.add_node(gid, isolated_node_name, isolated_namespace);
320-
ros_discovery_datawriter_->write(static_cast<void *>(&info));
313+
314+
auto it = micro_ros_graph_datawriters_.find(participant);
315+
if (it == micro_ros_graph_datawriters_.end())
316+
{
317+
// Create datawriter
318+
std::unique_ptr<eprosima::fastdds::dds::DataWriter> datawriter;
319+
datawriter.reset(publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos_));
320+
321+
it = micro_ros_graph_datawriters_.insert(
322+
std::make_pair(participant, std::move(datawriter))).first;
323+
}
324+
325+
it->second->write(static_cast<void *>(&info));
321326
}
322327
}
323328

324329
void GraphManager::remove_participant(
325-
const eprosima::fastrtps::rtps::GUID_t& guid)
330+
const eprosima::fastdds::dds::DomainParticipant* participant,
331+
bool from_microros)
326332
{
327-
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid);
333+
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid());
328334
graphCache_.remove_participant(gid);
335+
336+
if (from_microros)
337+
{
338+
rmw_dds_common::msg::ParticipantEntitiesInfo info;
339+
rmw_dds_common::convert_gid_to_msg(&gid, &info.gid);
340+
auto it = micro_ros_graph_datawriters_.find(participant);
341+
it->second->write(static_cast<void *>(&info));
342+
}
343+
micro_ros_graph_datawriters_.erase(participant);
329344
}
330345

331346
void GraphManager::add_datawriter(
@@ -438,7 +453,9 @@ void GraphManager::associate_entity(
438453
break;
439454
}
440455
}
441-
ros_discovery_datawriter_->write(static_cast<void *>(&info));
456+
457+
auto it = micro_ros_graph_datawriters_.find(participant);
458+
it->second->write(static_cast<void *>(&info));
442459
}
443460

444461

@@ -592,13 +609,13 @@ void GraphManager::ParticipantListener::on_participant_discovery(
592609
const std::string enclave =
593610
std::string(name_found->second.begin(), name_found->second.end());
594611

595-
graphManager_from_->add_participant(participant->guid(), info.info.m_participantName.to_string(), enclave);
612+
graphManager_from_->add_participant(participant, false, enclave);
596613
break;
597614
}
598615
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT:
599616
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT:
600617
{
601-
graphManager_from_->remove_participant(info.info.m_guid);
618+
graphManager_from_->remove_participant(participant, false);
602619
break;
603620
}
604621
default:
@@ -713,15 +730,13 @@ void GraphManager::ParticipantListener::on_subscriber_discovery(
713730
eprosima::fastrtps::rtps::ReaderDiscoveryInfo&& info)
714731
{
715732
process_discovery_info<eprosima::fastrtps::rtps::ReaderDiscoveryInfo>(info);
716-
// graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAREADER);
717733
}
718734

719735
void GraphManager::ParticipantListener::on_publisher_discovery(
720736
eprosima::fastdds::dds::DomainParticipant* /*participant*/,
721737
eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info)
722738
{
723739
process_discovery_info<eprosima::fastrtps::rtps::WriterDiscoveryInfo>(info);
724-
// graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAWRITER);
725740
}
726741

727742
GraphManager::DatareaderListener::DatareaderListener(

0 commit comments

Comments
 (0)
Back | FazBrowse Home | New Git URL