From e9bf7fa65be69ecdd37409079bb90f16e61137db Mon Sep 17 00:00:00 2001 From: Antonio cuadros Date: Thu, 22 Jul 2021 15:46:47 +0200 Subject: [PATCH] Update Graph manager destructor --- .../agent/graph_manager/graph_manager.hpp | 43 ++++++++++++++----- .../src/agent/graph_manager/graph_manager.cpp | 41 ++++++++++-------- 2 files changed, 56 insertions(+), 28 deletions(-) diff --git a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp index f0d94d2..b02c300 100644 --- a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -80,7 +80,30 @@ public: /** * @brief Default destructor. */ - ~GraphManager() = default; + ~GraphManager() + { + graphCache_.clear_on_change_callback(); + + exit = true; + cv_.notify_one(); + + if (microros_graph_publisher_.joinable()) + { + microros_graph_publisher_.join(); + } + + subscriber_->delete_datareader(ros_discovery_datareader_); + publisher_->delete_datawriter(ros_to_microros_graph_datawriter_); + + participant_->delete_subscriber(subscriber_); + participant_->delete_publisher(publisher_); + + // Delete topics + participant_->delete_topic(ros_discovery_topic_); + participant_->delete_topic(ros_to_microros_graph_topic_); + + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant(participant_); + } /** * @brief Implementation of the notification logic that updates the micro-ROS graph. @@ -274,6 +297,7 @@ private: std::thread microros_graph_publisher_; std::mutex mtx_; std::condition_variable cv_; + volatile bool exit = false; eprosima::fastdds::dds::DataWriterQos datawriter_qos_; @@ -283,18 +307,17 @@ private: std::unique_ptr participant_info_typesupport_; std::unique_ptr microros_graph_info_typesupport_; - std::unique_ptr participant_; - std::unique_ptr publisher_; - std::unique_ptr subscriber_; - std::unique_ptr ros_discovery_topic_; - std::unique_ptr ros_to_microros_graph_topic_; - std::unique_ptr ros_to_microros_graph_datawriter_; - std::unique_ptr ros_discovery_datareader_; - + eprosima::fastdds::dds::DomainParticipant* participant_; + eprosima::fastdds::dds::Publisher* publisher_; + eprosima::fastdds::dds::Subscriber* subscriber_; + eprosima::fastdds::dds::Topic* ros_discovery_topic_; + eprosima::fastdds::dds::Topic* ros_to_microros_graph_topic_; + eprosima::fastdds::dds::DataWriter* ros_to_microros_graph_datawriter_; + eprosima::fastdds::dds::DataReader* ros_discovery_datareader_; // Store a auxiliary publishers and datawriter for each participant created in micro-ROS std::map< const eprosima::fastdds::dds::DomainParticipant*, - std::unique_ptr + eprosima::fastdds::dds::DataWriter* > micro_ros_graph_datawriters_; }; diff --git a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp index 9badffc..8c5809a 100644 --- a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp @@ -54,29 +54,29 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; eprosima::fastdds::dds::StatusMask par_mask = eprosima::fastdds::dds::StatusMask::none(); - participant_.reset(eprosima::fastdds::dds::DomainParticipantFactory::get_instance()-> - create_participant(domain_id_, participant_qos, participant_listener_.get(), par_mask)); + participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()-> + create_participant(domain_id_, participant_qos, participant_listener_.get(), par_mask); // Register participant within typesupport participant_->register_type(*participant_info_typesupport_); participant_->register_type(*microros_graph_info_typesupport_); // Create publisher - publisher_.reset(participant_->create_publisher( - eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT)); + publisher_ = participant_->create_publisher( + eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT); // Create subscriber - subscriber_.reset(participant_->create_subscriber( - eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT)); + subscriber_ = participant_->create_subscriber( + eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT); // Create topics - ros_discovery_topic_.reset(participant_->create_topic("ros_discovery_info", + ros_discovery_topic_ = participant_->create_topic("ros_discovery_info", participant_info_typesupport_->get_type_name(), - eprosima::fastdds::dds::TOPIC_QOS_DEFAULT)); + eprosima::fastdds::dds::TOPIC_QOS_DEFAULT); - ros_to_microros_graph_topic_.reset(participant_->create_topic("ros_to_microros_graph", + ros_to_microros_graph_topic_ = participant_->create_topic("ros_to_microros_graph", microros_graph_info_typesupport_->get_type_name(), - eprosima::fastdds::dds::TOPIC_QOS_DEFAULT)); + eprosima::fastdds::dds::TOPIC_QOS_DEFAULT); // Create datawriters datawriter_qos_ = @@ -97,8 +97,8 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) eprosima::fastdds::dds::DataWriterQos ros_to_microros_datawriter_qos_ = datawriter_qos_; ros_to_microros_datawriter_qos_.history().kind = eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS; - ros_to_microros_graph_datawriter_.reset( - publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), ros_to_microros_datawriter_qos_)); + ros_to_microros_graph_datawriter_ = + publisher_->create_datawriter(ros_to_microros_graph_topic_, ros_to_microros_datawriter_qos_); // Create datareaders @@ -114,9 +114,9 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id) datareader_qos.durability().kind = eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS; - ros_discovery_datareader_.reset( - subscriber_->create_datareader(ros_discovery_topic_.get(), - datareader_qos, datareader_listener_.get())); + ros_discovery_datareader_ = + subscriber_->create_datareader(ros_discovery_topic_, + datareader_qos, datareader_listener_.get()); // Set graph cache on change callback function graphCache_.set_on_change_callback([this]() @@ -137,10 +137,15 @@ inline void GraphManager::publish_microros_graph() std::unique_lock lock(mtx_); cv_.wait(lock, [this]() { - return this->graph_changed_; + return this->graph_changed_ || exit; }); } + if (exit) + { + break; + } + if (display_on_change_) { std::cout << "Updated uros Graph: graph changed" << std::endl; @@ -315,8 +320,8 @@ void GraphManager::add_participant( if (it == micro_ros_graph_datawriters_.end()) { // Create datawriter - std::unique_ptr datawriter; - datawriter.reset(publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos_)); + eprosima::fastdds::dds::DataWriter* datawriter; + datawriter = publisher_->create_datawriter(ros_discovery_topic_, datawriter_qos_); it = micro_ros_graph_datawriters_.insert( std::make_pair(participant, std::move(datawriter))).first;