From 726692af04bc4fc3f671ce85444a5f515852fc8c Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 22 Mar 2022 10:35:14 +0100 Subject: [PATCH] Add services to graph manager (backport #127) (#128) * Add services to graph manager (#127) Signed-off-by: Pablo Garrido (cherry picked from commit 6d67807737da807e8db8b9952da04afaccf9de9d) * Revert "Add services to graph manager (#127)" This reverts commit 3738c3b1214af47f89b4f2d1dffc552b11441094. * Fix incompatibilities Signed-off-by: Pablo Garrido Co-authored-by: Pablo Garrido --- micro_ros_agent/src/agent/Agent.cpp | 143 ++++++++++++++++++++++++++++ 1 file changed, 143 insertions(+) diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 2a35e9b..c71d972 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -169,6 +169,149 @@ 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()); + + // TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dw = + datawriter->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dw); + + 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_dr = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dr); + 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()); + + // TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dw = + datawriter->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dw); + 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_dr = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dr); + 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()); + + // TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dw = + datawriter->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dw); + 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_dr = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dr); + 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()); + + // TODO(pablogs): Workaround for Fast-DDS bug #9977. Remove when fixed + const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle_dw = + datawriter->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datawriter_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dw); + 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_dr = + datareader->get_instance_handle(); + const eprosima::fastrtps::rtps::GUID_t datareader_guid = + eprosima::fastrtps::rtps::iHandle2GUID(instance_handle_dr); + 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;