From 3738c3b1214af47f89b4f2d1dffc552b11441094 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Tue, 22 Mar 2022 10:22:15 +0100 Subject: [PATCH 1/3] 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; From 42678a1c7c52c5b512852ea0621d9a031147673d Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Tue, 22 Mar 2022 10:22:15 +0100 Subject: [PATCH 2/3] Revert "Add services to graph manager (#127)" This reverts commit 3738c3b1214af47f89b4f2d1dffc552b11441094. --- micro_ros_agent/src/agent/Agent.cpp | 138 +++------------------------- 1 file changed, 13 insertions(+), 125 deletions(-) diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 12995d3..2a35e9b 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -79,9 +79,14 @@ bool Agent::create( { auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - graph_manager_->add_datawriter(datawriter->guid(), participant, datawriter); + // 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_->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, @@ -101,7 +106,12 @@ bool Agent::create( auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - graph_manager_->remove_datawriter(datawriter->guid()); + // 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); }); xrce_dds_agent_instance_.add_middleware_callback( @@ -159,128 +169,6 @@ 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; From 16be2597ef868e02839c851425fb9c0ad133373f Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Tue, 22 Mar 2022 10:31:30 +0100 Subject: [PATCH 3/3] Fix incompatibilities Signed-off-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;