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.
*/
~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<eprosima::fastdds::dds::TypeSupport> participant_info_typesupport_;
std::unique_ptr<eprosima::fastdds::dds::TypeSupport> microros_graph_info_typesupport_;
std::unique_ptr<eprosima::fastdds::dds::DomainParticipant> participant_;
std::unique_ptr<eprosima::fastdds::dds::Publisher> publisher_;
std::unique_ptr<eprosima::fastdds::dds::Subscriber> subscriber_;
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_discovery_topic_;
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_to_microros_graph_topic_;
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_to_microros_graph_datawriter_;
std::unique_ptr<eprosima::fastdds::dds::DataReader> 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>
eprosima::fastdds::dds::DataWriter*
> 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::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<std::mutex> 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<eprosima::fastdds::dds::DataWriter> 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;