diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp index 6860249..f27f4d2 100644 --- a/micro_ros_agent/include/agent/Agent.hpp +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -26,11 +26,21 @@ namespace agent { class Agent { -public: - +private: Agent(); - ~Agent() = default; + Agent(const Agent &) = delete; + + Agent(Agent &&) = delete; + + Agent& operator =( + const Agent &) = delete; + + Agent& operator =( + Agent &&) = delete; + +public: + static Agent& getInstance(); bool create( int argc, diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp index 9119838..0dabe24 100644 --- a/micro_ros_agent/src/agent/Agent.cpp +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -23,152 +23,154 @@ namespace agent { Agent::Agent() : xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance()) { + /** + * Add CREATE_PARTICIPANT callback. + */ + std::function on_create_participant + ([&]( + const eprosima::fastdds::dds::DomainParticipant* participant) -> void + { + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + 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 + { + auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); + graph_manager_->remove_participant(participant); + }); + 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 + { + 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_->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 + { + + 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); + }); + + 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 + { + 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 = + 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 + { + 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 = + 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)); +} + +Agent& Agent::getInstance() +{ + static Agent instance; + return instance; } bool Agent::create( int argc, char** argv) { - bool result = xrce_dds_agent_instance_.create(argc, argv); - if (result) - { - /** - * Add CREATE_PARTICIPANT callback. - */ - std::function on_create_participant - ([&]( - const eprosima::fastdds::dds::DomainParticipant* participant) -> void - { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - 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 - { - auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id()); - graph_manager_->remove_participant(participant); - }); - 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 - { - 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_->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 - { - - 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); - }); - - 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 - { - 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 = - 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 - { - 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 = - 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; + stop(); + return xrce_dds_agent_instance_.create(argc, argv); } void Agent::run() @@ -179,12 +181,12 @@ void Agent::run() void Agent::stop() { xrce_dds_agent_instance_.stop(); + graph_manager_map_.clear(); } std::shared_ptr Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id) { - -auto it = graph_manager_map_.find(domain_id); + auto it = graph_manager_map_.find(domain_id); if (it != graph_manager_map_.end()) { return it->second; diff --git a/micro_ros_agent/src/main.cpp b/micro_ros_agent/src/main.cpp index d5b29bf..0c4937c 100644 --- a/micro_ros_agent/src/main.cpp +++ b/micro_ros_agent/src/main.cpp @@ -16,7 +16,7 @@ int main(int argc, char** argv) { - uros::agent::Agent micro_ros_agent; + uros::agent::Agent& micro_ros_agent = micro_ros_agent.getInstance(); /** Bypass '--ros-args' flag, as we use our own CLI parser. * As a workaround for launch files, arguments will be passed from