From e9330bf97445146f0c5feafd85e082bf9ee7f0e0 Mon Sep 17 00:00:00 2001 From: Jose Antonio Moral Date: Wed, 2 Dec 2020 16:51:22 +0100 Subject: [PATCH] Create graph manager after checking passed CLI arguments and launch xrce-dds server (#41) --- micro_ros_agent/src/agent/Agent.cpp | 257 ++++++++++++++-------------- 1 file changed, 132 insertions(+), 125 deletions(-) diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 8a4261b..79f9d9e 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -22,138 +22,145 @@ namespace agent { Agent::Agent() : xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance()) - , graph_manager_(std::make_unique()) + , graph_manager_(nullptr) { - /** - * Add CREATE_PARTICIPANT callback. - */ - std::function on_create_participant - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void - { - graph_manager_->add_participant(participant); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT, - std::move(on_create_participant)); - - /** - * Add REMOVE_PARTICIPANT callback. - */ - std::function on_delete_participant - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void - { - graph_manager_->remove_participant(participant->guid()); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT, - std::move(on_delete_participant)); - - /** - * Add CREATE_DATAWRITER callback. - */ - std::function on_create_datawriter - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant, - const eprosima::fastdds::dds::DataWriter* datawriter) -> void - { - // 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); - }); - xrce_dds_agent_instance_.add_middleware_callback( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER, - std::move(on_create_datawriter)); - - /** - * Add DELETE_DATAWRITER callback. - */ - std::function on_delete_datawriter - ([&]( - const eprosima::fastdds::dds::DomainParticipant* /*participant*/, - const eprosima::fastdds::dds::DataWriter* datawriter) -> void - { - // 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( - eprosima::uxr::Middleware::Kind::FASTDDS, - eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER, - std::move(on_delete_datawriter)); - - /** - * Add CREATE_DATAREADER callback. - */ - std::function on_create_datareader - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant, - const eprosima::fastdds::dds::DataReader* datareader) -> void - { - // TODO(jamoralp): 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_DATAREADER, - std::move(on_create_datareader)); - - /** - * Add DELETE_DATAREADER callback. - */ - std::function on_delete_datareader - ([&]( - const eprosima::fastdds::dds::DomainParticipant* /*participant*/, - const eprosima::fastdds::dds::DataReader* datareader) -> void - { - // TODO(jamoralp): 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_DATAREADER, - std::move(on_delete_datareader)); } bool Agent::create( int argc, char** argv) { - return xrce_dds_agent_instance_.create(argc, argv); + bool result = xrce_dds_agent_instance_.create(argc, argv); + if (result) + { + graph_manager_.reset(new graph_manager::GraphManager()); + + /** + * Add CREATE_PARTICIPANT callback. + */ + std::function on_create_participant + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant) -> void + { + graph_manager_->add_participant(participant); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT, + std::move(on_create_participant)); + + /** + * Add REMOVE_PARTICIPANT callback. + */ + std::function on_delete_participant + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant) -> void + { + graph_manager_->remove_participant(participant->guid()); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT, + std::move(on_delete_participant)); + + /** + * Add CREATE_DATAWRITER callback. + */ + std::function on_create_datawriter + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataWriter* datawriter) -> void + { + // 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); + }); + xrce_dds_agent_instance_.add_middleware_callback( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER, + std::move(on_create_datawriter)); + + /** + * Add DELETE_DATAWRITER callback. + */ + std::function on_delete_datawriter + ([&]( + const eprosima::fastdds::dds::DomainParticipant* /*participant*/, + const eprosima::fastdds::dds::DataWriter* datawriter) -> void + { + // 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( + eprosima::uxr::Middleware::Kind::FASTDDS, + eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER, + std::move(on_delete_datawriter)); + + /** + * Add CREATE_DATAREADER callback. + */ + std::function on_create_datareader + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataReader* datareader) -> void + { + // TODO(jamoralp): 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_DATAREADER, + std::move(on_create_datareader)); + + /** + * Add DELETE_DATAREADER callback. + */ + std::function on_delete_datareader + ([&]( + const eprosima::fastdds::dds::DomainParticipant* /*participant*/, + const eprosima::fastdds::dds::DataReader* datareader) -> void + { + // TODO(jamoralp): 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_DATAREADER, + std::move(on_delete_datareader)); + } + + return result; } void Agent::run()