Skip to content

Commit ed0402c

Browse files
mergify[bot]pablogs9
mergify[bot]
and
pablogs9
authored
Add services to graph manager (#127) (#129)
Signed-off-by: Pablo Garrido <[email protected]> (cherry picked from commit 6d67807) Co-authored-by: Pablo Garrido <[email protected]>
1 parent 5780038 commit ed0402c

File tree

1 file changed

+125
-13
lines changed

1 file changed

+125
-13
lines changed

‎micro_ros_agent/src/agent/Agent.cpp‎

Lines changed: 125 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -79,14 +79,9 @@ bool Agent::create(
7979
{
8080
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
8181

82-
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
83-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
84-
datawriter->get_instance_handle();
85-
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
86-
iHandle2GUID(instance_handle);
87-
graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
82+
graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter);
8883
graph_manager_->associate_entity(
89-
datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER);
84+
datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER);
9085
});
9186
xrce_dds_agent_instance_.add_middleware_callback(
9287
eprosima::uxr::Middleware::Kind::FASTDDS,
@@ -106,12 +101,7 @@ bool Agent::create(
106101

107102
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
108103

109-
// TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
110-
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
111-
datawriter->get_instance_handle();
112-
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
113-
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
114-
graph_manager_->remove_datawriter(datawriter_guid);
104+
graph_manager_->remove_datawriter(datawriter->guid());
115105
});
116106

117107
xrce_dds_agent_instance_.add_middleware_callback(
@@ -169,6 +159,128 @@ bool Agent::create(
169159
eprosima::uxr::Middleware::Kind::FASTDDS,
170160
eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER,
171161
std::move(on_delete_datareader));
162+
163+
/**
164+
* Add CREATE_REQUESTER callback.
165+
*/
166+
std::function<void (
167+
const eprosima::fastdds::dds::DomainParticipant *,
168+
const eprosima::fastdds::dds::DataWriter *,
169+
const eprosima::fastdds::dds::DataReader *)> on_create_requester
170+
([&](
171+
const eprosima::fastdds::dds::DomainParticipant* participant,
172+
const eprosima::fastdds::dds::DataWriter* datawriter,
173+
const eprosima::fastdds::dds::DataReader * datareader) -> void
174+
{
175+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
176+
177+
graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter);
178+
graph_manager_->associate_entity(
179+
datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER);
180+
181+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
182+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
183+
datareader->get_instance_handle();
184+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
185+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
186+
graph_manager_->add_datareader(datareader_guid, participant, datareader);
187+
graph_manager_->associate_entity(
188+
datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
189+
});
190+
xrce_dds_agent_instance_.add_middleware_callback(
191+
eprosima::uxr::Middleware::Kind::FASTDDS,
192+
eprosima::uxr::middleware::CallbackKind::CREATE_REQUESTER,
193+
std::move(on_create_requester));
194+
195+
/**
196+
* Add DELETE_REQUESTER callback.
197+
*/
198+
std::function<void (
199+
const eprosima::fastdds::dds::DomainParticipant *,
200+
const eprosima::fastdds::dds::DataWriter *,
201+
const eprosima::fastdds::dds::DataReader *)> on_delete_requester
202+
([&](
203+
const eprosima::fastdds::dds::DomainParticipant* participant,
204+
const eprosima::fastdds::dds::DataWriter* datawriter,
205+
const eprosima::fastdds::dds::DataReader * datareader) -> void
206+
{
207+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
208+
209+
graph_manager_->remove_datawriter(datawriter->guid());
210+
211+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
212+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
213+
datareader->get_instance_handle();
214+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
215+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
216+
graph_manager_->remove_datareader(datareader_guid);
217+
});
218+
219+
xrce_dds_agent_instance_.add_middleware_callback(
220+
eprosima::uxr::Middleware::Kind::FASTDDS,
221+
eprosima::uxr::middleware::CallbackKind::DELETE_REQUESTER,
222+
std::move(on_delete_requester));
223+
224+
/**
225+
* Add CREATE_REPLIER callback.
226+
*/
227+
std::function<void (
228+
const eprosima::fastdds::dds::DomainParticipant *,
229+
const eprosima::fastdds::dds::DataWriter *,
230+
const eprosima::fastdds::dds::DataReader *)> on_create_replier
231+
([&](
232+
const eprosima::fastdds::dds::DomainParticipant* participant,
233+
const eprosima::fastdds::dds::DataWriter* datawriter,
234+
const eprosima::fastdds::dds::DataReader * datareader) -> void
235+
{
236+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
237+
238+
graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter);
239+
graph_manager_->associate_entity(
240+
datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER);
241+
242+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
243+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
244+
datareader->get_instance_handle();
245+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
246+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
247+
graph_manager_->add_datareader(datareader_guid, participant, datareader);
248+
graph_manager_->associate_entity(
249+
datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
250+
});
251+
xrce_dds_agent_instance_.add_middleware_callback(
252+
eprosima::uxr::Middleware::Kind::FASTDDS,
253+
eprosima::uxr::middleware::CallbackKind::CREATE_REPLIER,
254+
std::move(on_create_replier));
255+
256+
/**
257+
* Add DELETE_REPLIER callback.
258+
*/
259+
std::function<void (
260+
const eprosima::fastdds::dds::DomainParticipant *,
261+
const eprosima::fastdds::dds::DataWriter *,
262+
const eprosima::fastdds::dds::DataReader *)> on_delete_replier
263+
([&](
264+
const eprosima::fastdds::dds::DomainParticipant* participant,
265+
const eprosima::fastdds::dds::DataWriter* datawriter,
266+
const eprosima::fastdds::dds::DataReader * datareader) -> void
267+
{
268+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
269+
270+
graph_manager_->remove_datawriter(datawriter->guid());
271+
272+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
273+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
274+
datareader->get_instance_handle();
275+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
276+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
277+
graph_manager_->remove_datareader(datareader_guid);
278+
});
279+
280+
xrce_dds_agent_instance_.add_middleware_callback(
281+
eprosima::uxr::Middleware::Kind::FASTDDS,
282+
eprosima::uxr::middleware::CallbackKind::DELETE_REPLIER,
283+
std::move(on_delete_replier));
172284
}
173285

174286
return result;

0 commit comments

Comments
 (0)