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

Fix graph manager datawriters · micro-ROS/micro-ROS-Agent@2847a2e · GitHub

/ micro-ROS-Agent Public

Commit 2847a2e

Browse files
pablogs9
committed
Fix graph manager datawriters
Fixes Fix Fix destruction
1 parent 95fecd7 commit 2847a2e

3 files changed

Lines changed: 119 additions & 121 deletions

File tree

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

Lines changed: 20 additions & 17 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+
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+
eprosima::fastdds::dds::DomainParticipant* participant,
107+
bool from_microros = true);
114108

115109
/**
116110
* @brief Adds a DDS datawriter to the graph tree.
@@ -186,7 +180,7 @@ class GraphManager
186180
*/
187181
void associate_entity(
188182
const eprosima::fastrtps::rtps::GUID_t& entity_guid,
189-
const eprosima::fastdds::dds::DomainParticipant* participant,
183+
eprosima::fastdds::dds::DomainParticipant* participant,
190184
const dds::xrce::ObjectKind& entity_kind);
191185

192186
private:
@@ -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,17 @@ 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+
eprosima::fastdds::dds::DomainParticipant*,
297+
std::pair<
298+
std::unique_ptr<eprosima::fastdds::dds::Publisher>,
299+
std::unique_ptr<eprosima::fastdds::dds::DataWriter>
300+
>
301+
> micro_ros_graph_datawriters_;
299302
};
300303

301304
} // namespace graph_manager

‎micro_ros_agent/src/agent/Agent.cpp‎

Lines changed: 39 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -32,20 +32,16 @@ bool Agent::create(
3232
bool result = xrce_dds_agent_instance_.create(argc, argv);
3333
if (result)
3434
{
35+
auto graph_manager_ = find_or_create_graph_manager(0);
3536
/**
3637
* Add CREATE_PARTICIPANT callback.
3738
*/
3839
std::function<void (
39-
const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
40+
eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
4041
([&](
41-
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
42+
eprosima::fastdds::dds::DomainParticipant* participant) -> void
4243
{
43-
auto graph_manager_ =
44-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
45-
participant->get_domain_id()
46-
)
47-
);
48-
44+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
4945
graph_manager_->add_participant(participant);
5046
});
5147
xrce_dds_agent_instance_.add_middleware_callback(
@@ -57,17 +53,12 @@ bool Agent::create(
5753
* Add REMOVE_PARTICIPANT callback.
5854
*/
5955
std::function<void (
60-
const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
56+
eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
6157
([&](
62-
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
58+
eprosima::fastdds::dds::DomainParticipant* participant) -> void
6359
{
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());
60+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
61+
graph_manager_->remove_participant(participant);
7162
});
7263
xrce_dds_agent_instance_.add_middleware_callback(
7364
eprosima::uxr::Middleware::Kind::FASTDDS,
@@ -78,22 +69,18 @@ bool Agent::create(
7869
* Add CREATE_DATAWRITER callback.
7970
*/
8071
std::function<void (
81-
const eprosima::fastdds::dds::DomainParticipant *,
82-
const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
72+
eprosima::fastdds::dds::DomainParticipant *,
73+
eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
8374
([&](
84-
const eprosima::fastdds::dds::DomainParticipant* participant,
85-
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
75+
eprosima::fastdds::dds::DomainParticipant* participant,
76+
eprosima::fastdds::dds::DataWriter* datawriter) -> void
8677
{
87-
auto graph_manager_ =
88-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
89-
participant->get_domain_id()
90-
)
91-
);
78+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
9279

9380
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
94-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
81+
eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
9582
datawriter->get_instance_handle();
96-
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
83+
eprosima::fastrtps::rtps::GUID_t datawriter_guid =
9784
iHandle2GUID(instance_handle);
9885
graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
9986
graph_manager_->associate_entity(
@@ -108,23 +95,19 @@ bool Agent::create(
10895
* Add DELETE_DATAWRITER callback.
10996
*/
11097
std::function<void (
111-
const eprosima::fastdds::dds::DomainParticipant *,
112-
const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
98+
eprosima::fastdds::dds::DomainParticipant *,
99+
eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
113100
([&](
114-
const eprosima::fastdds::dds::DomainParticipant* participant,
115-
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
101+
eprosima::fastdds::dds::DomainParticipant* participant,
102+
eprosima::fastdds::dds::DataWriter* datawriter) -> void
116103
{
117104

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

124107
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
125-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
108+
eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
126109
datawriter->get_instance_handle();
127-
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
110+
eprosima::fastrtps::rtps::GUID_t datawriter_guid =
128111
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
129112
graph_manager_->remove_datawriter(datawriter_guid);
130113
});
@@ -138,22 +121,18 @@ bool Agent::create(
138121
* Add CREATE_DATAREADER callback.
139122
*/
140123
std::function<void (
141-
const eprosima::fastdds::dds::DomainParticipant *,
142-
const eprosima::fastdds::dds::DataReader*)> on_create_datareader
124+
eprosima::fastdds::dds::DomainParticipant *,
125+
eprosima::fastdds::dds::DataReader*)> on_create_datareader
143126
([&](
144-
const eprosima::fastdds::dds::DomainParticipant* participant,
145-
const eprosima::fastdds::dds::DataReader* datareader) -> void
127+
eprosima::fastdds::dds::DomainParticipant* participant,
128+
eprosima::fastdds::dds::DataReader* datareader) -> void
146129
{
147-
auto graph_manager_ =
148-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
149-
participant->get_domain_id()
150-
)
151-
);
130+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
152131

153132
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
154-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
133+
eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
155134
datareader->get_instance_handle();
156-
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
135+
eprosima::fastrtps::rtps::GUID_t datareader_guid =
157136
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
158137
graph_manager_->add_datareader(datareader_guid, participant, datareader);
159138
graph_manager_->associate_entity(
@@ -168,22 +147,18 @@ bool Agent::create(
168147
* Add DELETE_DATAREADER callback.
169148
*/
170149
std::function<void (
171-
const eprosima::fastdds::dds::DomainParticipant *,
172-
const eprosima::fastdds::dds::DataReader *)> on_delete_datareader
150+
eprosima::fastdds::dds::DomainParticipant *,
151+
eprosima::fastdds::dds::DataReader *)> on_delete_datareader
173152
([&](
174-
const eprosima::fastdds::dds::DomainParticipant* participant,
175-
const eprosima::fastdds::dds::DataReader* datareader) -> void
153+
eprosima::fastdds::dds::DomainParticipant* participant,
154+
eprosima::fastdds::dds::DataReader* datareader) -> void
176155
{
177-
auto graph_manager_ =
178-
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
179-
participant->get_domain_id()
180-
)
181-
);
156+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
182157

183158
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
184-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
159+
eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
185160
datareader->get_instance_handle();
186-
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
161+
eprosima::fastrtps::rtps::GUID_t datareader_guid =
187162
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
188163
graph_manager_->remove_datareader(datareader_guid);
189164
});
@@ -204,16 +179,14 @@ void Agent::run()
204179

205180
std::shared_ptr<graph_manager::GraphManager> Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id)
206181
{
207-
auto it = graph_manager_map_.find(domain_id);
182+
183+
auto it = graph_manager_map_.find(domain_id);
208184

209185
if (it != graph_manager_map_.end()) {
210186
return it->second;
211187
}else{
212188
return graph_manager_map_.insert(
213-
std::pair<
214-
eprosima::fastdds::dds::DomainId_t,
215-
std::shared_ptr<graph_manager::GraphManager>
216-
>(
189+
std::make_pair(
217190
domain_id,
218191
std::make_shared<graph_manager::GraphManager>(domain_id)
219192
)

0 commit comments

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