From 57d7d16b89cf737944d60ba7cc0422f08019b395 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Tue, 22 Mar 2022 10:22:15 +0100 Subject: [PATCH] Add services to graph manager (#127) Signed-off-by: Pablo Garrido (cherry picked from commit 6d67807737da807e8db8b9952da04afaccf9de9d) --- micro_ros_agent/src/agent/Agent.cpp | 138 +++++++++++++++++++++++++--- 1 file changed, 125 insertions(+), 13 deletions(-) diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 2a35e9b..12995d3 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -79,14 +79,9 @@ bool Agent::create( { auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datawriter->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datawriter_guid = - iHandle2GUID(instance_handle); - graph_manager_->add_datawriter(datawriter_guid, participant, datawriter); + graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter); graph_manager_->associate_entity( - datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER); + datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER); }); xrce_dds_agent_instance_.add_middleware_callback( eprosima::uxr::Middleware::Kind::FASTDDS, @@ -106,12 +101,7 @@ bool Agent::create( auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed - const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = - datawriter->get_instance_handle(); - const eprosima::fastrtps::rtps::GUID_t datawriter_guid = - eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); - graph_manager_->remove_datawriter(datawriter_guid); + graph_manager_->remove_datawriter(datawriter->guid()); }); xrce_dds_agent_instance_.add_middleware_callback( @@ -169,6 +159,128 @@ bool Agent::create( eprosima::uxr::Middleware::Kind::FASTDDS, eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER, std::move(on_delete_datareader)); + + /** + * Add CREATE_REQUESTER callback. + */ + std::function on_create_requester + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataWriter* datawriter, + const eprosima::fastdds::dds::DataReader * datareader) -> void + { + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + + graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter); + graph_manager_->associate_entity( + datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER); + + // TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); + graph_manager_->add_datareader(datareader_guid, participant, datareader); + graph_manager_->associate_entity( + datareader_guid, participant, dds::xrce::OBJK_DATAREADER); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_REQUESTER, + std::move(on_create_requester)); + + /** + * Add DELETE_REQUESTER callback. + */ + std::function on_delete_requester + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataWriter* datawriter, + const eprosima::fastdds::dds::DataReader * datareader) -> void + { + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + + graph_manager_->remove_datawriter(datawriter->guid()); + + // TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); + graph_manager_->remove_datareader(datareader_guid); + }); + + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::DELETE_REQUESTER, + std::move(on_delete_requester)); + + /** + * Add CREATE_REPLIER callback. + */ + std::function on_create_replier + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataWriter* datawriter, + const eprosima::fastdds::dds::DataReader * datareader) -> void + { + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + + graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter); + graph_manager_->associate_entity( + datawriter->guid(), participant, dds::xrce::OBJK_DATAWRITER); + + // TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); + graph_manager_->add_datareader(datareader_guid, participant, datareader); + graph_manager_->associate_entity( + datareader_guid, participant, dds::xrce::OBJK_DATAREADER); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_REPLIER, + std::move(on_create_replier)); + + /** + * Add DELETE_REPLIER callback. + */ + std::function on_delete_replier + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataWriter* datawriter, + const eprosima::fastdds::dds::DataReader * datareader) -> void + { + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + + graph_manager_->remove_datawriter(datawriter->guid()); + + // TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle); + graph_manager_->remove_datareader(datareader_guid); + }); + + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::DELETE_REPLIER, + std::move(on_delete_replier)); } return result;