Skip to content

Commit 726692a

Browse files
mergify[bot]pablogs9
mergify[bot]
and
pablogs9
authored
Add services to graph manager (backport #127) (#128)
* Add services to graph manager (#127) Signed-off-by: Pablo Garrido <pablogs9@gmail.com> (cherry picked from commit 6d67807) * Revert "Add services to graph manager (#127)" This reverts commit 3738c3b. * Fix incompatibilities Signed-off-by: Pablo Garrido <pablogs9@gmail.com> Co-authored-by: Pablo Garrido <pablogs9@gmail.com>
1 parent 2326a20 commit 726692a

1 file changed

Lines changed: 143 additions & 0 deletions

File tree

‎micro_ros_agent/src/agent/Agent.cpp‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -169,6 +169,149 @@ bool Agent::create(
169169
eprosima::uxr::Middleware::Kind::FASTDDS,
170170
eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER,
171171
std::move(on_delete_datareader));
172+
173+
/**
174+
* Add CREATE_REQUESTER callback.
175+
*/
176+
std::function<void (
177+
const eprosima::fastdds::dds::DomainParticipant *,
178+
const eprosima::fastdds::dds::DataWriter *,
179+
const eprosima::fastdds::dds::DataReader *)> on_create_requester
180+
([&](
181+
const eprosima::fastdds::dds::DomainParticipant* participant,
182+
const eprosima::fastdds::dds::DataWriter* datawriter,
183+
const eprosima::fastdds::dds::DataReader * datareader) -> void
184+
{
185+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
186+
187+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
188+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dw =
189+
datawriter->get_instance_handle();
190+
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
191+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dw);
192+
193+
graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
194+
graph_manager_->associate_entity(
195+
datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER);
196+
197+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
198+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dr =
199+
datareader->get_instance_handle();
200+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
201+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dr);
202+
graph_manager_->add_datareader(datareader_guid, participant, datareader);
203+
graph_manager_->associate_entity(
204+
datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
205+
});
206+
xrce_dds_agent_instance_.add_middleware_callback(
207+
eprosima::uxr::Middleware::Kind::FASTDDS,
208+
eprosima::uxr::middleware::CallbackKind::CREATE_REQUESTER,
209+
std::move(on_create_requester));
210+
211+
/**
212+
* Add DELETE_REQUESTER callback.
213+
*/
214+
std::function<void (
215+
const eprosima::fastdds::dds::DomainParticipant *,
216+
const eprosima::fastdds::dds::DataWriter *,
217+
const eprosima::fastdds::dds::DataReader *)> on_delete_requester
218+
([&](
219+
const eprosima::fastdds::dds::DomainParticipant* participant,
220+
const eprosima::fastdds::dds::DataWriter* datawriter,
221+
const eprosima::fastdds::dds::DataReader * datareader) -> void
222+
{
223+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
224+
225+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
226+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dw =
227+
datawriter->get_instance_handle();
228+
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
229+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dw);
230+
graph_manager_->remove_datawriter(datawriter_guid);
231+
232+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
233+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dr =
234+
datareader->get_instance_handle();
235+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
236+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dr);
237+
graph_manager_->remove_datareader(datareader_guid);
238+
});
239+
240+
xrce_dds_agent_instance_.add_middleware_callback(
241+
eprosima::uxr::Middleware::Kind::FASTDDS,
242+
eprosima::uxr::middleware::CallbackKind::DELETE_REQUESTER,
243+
std::move(on_delete_requester));
244+
245+
/**
246+
* Add CREATE_REPLIER callback.
247+
*/
248+
std::function<void (
249+
const eprosima::fastdds::dds::DomainParticipant *,
250+
const eprosima::fastdds::dds::DataWriter *,
251+
const eprosima::fastdds::dds::DataReader *)> on_create_replier
252+
([&](
253+
const eprosima::fastdds::dds::DomainParticipant* participant,
254+
const eprosima::fastdds::dds::DataWriter* datawriter,
255+
const eprosima::fastdds::dds::DataReader * datareader) -> void
256+
{
257+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
258+
259+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
260+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dw =
261+
datawriter->get_instance_handle();
262+
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
263+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dw);
264+
graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
265+
graph_manager_->associate_entity(
266+
datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER);
267+
268+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
269+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dr =
270+
datareader->get_instance_handle();
271+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
272+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dr);
273+
graph_manager_->add_datareader(datareader_guid, participant, datareader);
274+
graph_manager_->associate_entity(
275+
datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
276+
});
277+
xrce_dds_agent_instance_.add_middleware_callback(
278+
eprosima::uxr::Middleware::Kind::FASTDDS,
279+
eprosima::uxr::middleware::CallbackKind::CREATE_REPLIER,
280+
std::move(on_create_replier));
281+
282+
/**
283+
* Add DELETE_REPLIER callback.
284+
*/
285+
std::function<void (
286+
const eprosima::fastdds::dds::DomainParticipant *,
287+
const eprosima::fastdds::dds::DataWriter *,
288+
const eprosima::fastdds::dds::DataReader *)> on_delete_replier
289+
([&](
290+
const eprosima::fastdds::dds::DomainParticipant* participant,
291+
const eprosima::fastdds::dds::DataWriter* datawriter,
292+
const eprosima::fastdds::dds::DataReader * datareader) -> void
293+
{
294+
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
295+
296+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
297+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dw =
298+
datawriter->get_instance_handle();
299+
const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
300+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dw);
301+
graph_manager_->remove_datawriter(datawriter_guid);
302+
303+
// TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed
304+
const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dr =
305+
datareader->get_instance_handle();
306+
const eprosima::fastrtps::rtps::GUID_t datareader_guid =
307+
eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dr);
308+
graph_manager_->remove_datareader(datareader_guid);
309+
});
310+
311+
xrce_dds_agent_instance_.add_middleware_callback(
312+
eprosima::uxr::Middleware::Kind::FASTDDS,
313+
eprosima::uxr::middleware::CallbackKind::DELETE_REPLIER,
314+
std::move(on_delete_replier));
172315
}
173316

174317
return result;

0 commit comments

Comments
 (0)