nicco/micro-ROS-Agent
1
0
Fork 0
mirror of https://github.com/micro-ROS/micro-ROS-Agent.git synced 2026-09-06 18:05:17 +02:00
Code Issues Packages Projects Releases Wiki Activity

Update Graph manager destructor

Browse Source
This commit is contained in:
Antonio cuadros
2021-07-22 15:46:47 +02:00
parent 3eb34010f0
commit e9bf7fa65b
2 changed files with 56 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
micro_ros_agent/include/agent/graph_manager/graph_manager.hpp
+33 -10
View File
@@ -80,7 +80,30 @@ public:
/** /**
* @brief Default destructor. * @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. * @brief Implementation of the notification logic that updates the micro-ROS graph.
@@ -274,6 +297,7 @@ private:
std::thread microros_graph_publisher_; std::thread microros_graph_publisher_;
std::mutex mtx_; std::mutex mtx_;
std::condition_variable cv_; std::condition_variable cv_;
volatile bool exit = false;
eprosima::fastdds::dds::DataWriterQos datawriter_qos_; eprosima::fastdds::dds::DataWriterQos datawriter_qos_;
@@ -283,18 +307,17 @@ private:
std::unique_ptr<eprosima::fastdds::dds::TypeSupport> participant_info_typesupport_; std::unique_ptr<eprosima::fastdds::dds::TypeSupport> participant_info_typesupport_;
std::unique_ptr<eprosima::fastdds::dds::TypeSupport> microros_graph_info_typesupport_; std::unique_ptr<eprosima::fastdds::dds::TypeSupport> microros_graph_info_typesupport_;
std::unique_ptr<eprosima::fastdds::dds::DomainParticipant> participant_; eprosima::fastdds::dds::DomainParticipant* participant_;
std::unique_ptr<eprosima::fastdds::dds::Publisher> publisher_; eprosima::fastdds::dds::Publisher* publisher_;
std::unique_ptr<eprosima::fastdds::dds::Subscriber> subscriber_; eprosima::fastdds::dds::Subscriber* subscriber_;
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_discovery_topic_; eprosima::fastdds::dds::Topic* ros_discovery_topic_;
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_to_microros_graph_topic_; eprosima::fastdds::dds::Topic* ros_to_microros_graph_topic_;
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_to_microros_graph_datawriter_; eprosima::fastdds::dds::DataWriter* ros_to_microros_graph_datawriter_;
std::unique_ptr<eprosima::fastdds::dds::DataReader> ros_discovery_datareader_; eprosima::fastdds::dds::DataReader* ros_discovery_datareader_;
// Store a auxiliary publishers and datawriter for each participant created in micro-ROS // Store a auxiliary publishers and datawriter for each participant created in micro-ROS
std::map< std::map<
const eprosima::fastdds::dds::DomainParticipant*, const eprosima::fastdds::dds::DomainParticipant*,
std::unique_ptr<eprosima::fastdds::dds::DataWriter> eprosima::fastdds::dds::DataWriter*
> micro_ros_graph_datawriters_; > micro_ros_graph_datawriters_;
}; };
micro_ros_agent/src/agent/graph_manager/graph_manager.cpp
+23 -18
View File
@@ -54,29 +54,29 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
eprosima::fastdds::dds::StatusMask par_mask = eprosima::fastdds::dds::StatusMask::none(); eprosima::fastdds::dds::StatusMask par_mask = eprosima::fastdds::dds::StatusMask::none();
participant_.reset(eprosima::fastdds::dds::DomainParticipantFactory::get_instance()-> participant_ = eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->
create_participant(domain_id_, participant_qos, participant_listener_.get(), par_mask)); create_participant(domain_id_, participant_qos, participant_listener_.get(), par_mask);
// Register participant within typesupport // Register participant within typesupport
participant_->register_type(*participant_info_typesupport_); participant_->register_type(*participant_info_typesupport_);
participant_->register_type(*microros_graph_info_typesupport_); participant_->register_type(*microros_graph_info_typesupport_);
// Create publisher // Create publisher
publisher_.reset(participant_->create_publisher( publisher_ = participant_->create_publisher(
eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT)); eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT);
// Create subscriber // Create subscriber
subscriber_.reset(participant_->create_subscriber( subscriber_ = participant_->create_subscriber(
eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT)); eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT);
// Create topics // 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(), 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(), microros_graph_info_typesupport_->get_type_name(),
eprosima::fastdds::dds::TOPIC_QOS_DEFAULT)); eprosima::fastdds::dds::TOPIC_QOS_DEFAULT);
// Create datawriters // Create datawriters
datawriter_qos_ = 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_; eprosima::fastdds::dds::DataWriterQos ros_to_microros_datawriter_qos_ = datawriter_qos_;
ros_to_microros_datawriter_qos_.history().kind = ros_to_microros_datawriter_qos_.history().kind =
eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS; eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS;
ros_to_microros_graph_datawriter_.reset( ros_to_microros_graph_datawriter_ =
publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), ros_to_microros_datawriter_qos_)); publisher_->create_datawriter(ros_to_microros_graph_topic_, ros_to_microros_datawriter_qos_);
// Create datareaders // Create datareaders
@@ -114,9 +114,9 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
datareader_qos.durability().kind = datareader_qos.durability().kind =
eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS; eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
ros_discovery_datareader_.reset( ros_discovery_datareader_ =
subscriber_->create_datareader(ros_discovery_topic_.get(), subscriber_->create_datareader(ros_discovery_topic_,
datareader_qos, datareader_listener_.get())); datareader_qos, datareader_listener_.get());
// Set graph cache on change callback function // Set graph cache on change callback function
graphCache_.set_on_change_callback([this]() graphCache_.set_on_change_callback([this]()
@@ -137,10 +137,15 @@ inline void GraphManager::publish_microros_graph()
std::unique_lock<std::mutex> lock(mtx_); std::unique_lock<std::mutex> lock(mtx_);
cv_.wait(lock, [this]() cv_.wait(lock, [this]()
{ {
return this->graph_changed_; return this->graph_changed_ || exit;
}); });
} }
if (exit)
{
break;
}
if (display_on_change_) if (display_on_change_)
{ {
std::cout << "Updated uros Graph: graph changed" << std::endl; std::cout << "Updated uros Graph: graph changed" << std::endl;
@@ -315,8 +320,8 @@ void GraphManager::add_participant(
if (it == micro_ros_graph_datawriters_.end()) if (it == micro_ros_graph_datawriters_.end())
{ {
// Create datawriter // Create datawriter
std::unique_ptr<eprosima::fastdds::dds::DataWriter> datawriter; eprosima::fastdds::dds::DataWriter* datawriter;
datawriter.reset(publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos_)); datawriter = publisher_->create_datawriter(ros_discovery_topic_, datawriter_qos_);
it = micro_ros_graph_datawriters_.insert( it = micro_ros_graph_datawriters_.insert(
std::make_pair(participant, std::move(datawriter))).first; std::make_pair(participant, std::move(datawriter))).first;