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

Fix graph manager datawriters behaviour (#84) (#85)

Browse Source 
* Fix graph manager datawriters

Fixes

Fix

Fix destruction

* Revert "Fix graph manager datawriters"

This reverts commit 2847a2ebc3.

* Initial

(cherry picked from commit 646ca52aab)

Co-authored-by: Pablo Garrido <pablogs9@gmail.com>
This commit is contained in:
mergify[bot]
2021-07-09 13:41:31 +02:00
committed by GitHub
co-authored by Pablo Garrido
parent e43cde14ae
commit 256424b7ec
3 changed files with 78 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
micro_ros_agent/include/agent/graph_manager/graph_manager.hpp
+16 -16
View File
@@ -89,28 +89,22 @@ public:
/**
* @brief Adds a DDS participant to the graph tree.
* @param participant Pointer to the participant to be added to the graph.
*/
void add_participant(
const eprosima::fastdds::dds::DomainParticipant* participant);
/**
* @brief Adds a DDS participant to the graph tree.
* @param guid rtps::GUID_t of the participant to be added.
* @param node_name Name of the ROS 2 node associated to the given participant.
* @param participant eprosima::fastdds::dds::DomainParticipant to be added.
* @param from_microros if this participant has been added from micro-ROS.
* @param enclave ROS 2 enclave.
*/
void add_participant(
const eprosima::fastrtps::rtps::GUID_t& guid,
const std::string& node_name,
const std::string& enclave);
const eprosima::fastdds::dds::DomainParticipant* participant,
bool from_microros = true,
const std::string& enclave = "/");
/**
* @brief Removes a DDS participant from the graph tree.
* @param guid rtps::GUID_t of the participant to be removed.
* @param participant eprosima::fastdds::dds::DomainParticipant to be removed.
*/
void remove_participant(
const eprosima::fastrtps::rtps::GUID_t& guid);
const eprosima::fastdds::dds::DomainParticipant* participant,
bool from_microros = true);
/**
* @brief Adds a DDS datawriter to the graph tree.
@@ -277,11 +271,12 @@ private:
eprosima::fastdds::dds::DomainId_t domain_id_;
bool graph_changed_;
bool display_on_change_;
const char * enclave_;
std::thread microros_graph_publisher_;
std::mutex mtx_;
std::condition_variable cv_;
eprosima::fastdds::dds::DataWriterQos datawriter_qos_;
rmw_dds_common::GraphCache graphCache_;
std::unique_ptr<ParticipantListener> participant_listener_;
std::unique_ptr<DatareaderListener> datareader_listener_;
@@ -293,9 +288,14 @@ private:
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_discovery_datawriter_;
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_to_microros_graph_datawriter_;
std::unique_ptr<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_;
};
} // namespace graph_manager
micro_ros_agent/src/agent/Agent.cpp
+10 -38
View File
@@ -40,12 +40,7 @@ bool Agent::create(
([&](
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
{
auto graph_manager_ =
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
participant->get_domain_id()
)
);
auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
graph_manager_->add_participant(participant);
});
xrce_dds_agent_instance_.add_middleware_callback(
@@ -61,13 +56,8 @@ bool Agent::create(
([&](
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
{
auto graph_manager_ =
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
participant->get_domain_id()
)
);
graph_manager_->remove_participant(participant->guid());
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,
@@ -84,11 +74,7 @@ bool Agent::create(
const eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
{
auto graph_manager_ =
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
participant->get_domain_id()
)
);
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 =
@@ -115,11 +101,7 @@ bool Agent::create(
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
{
auto graph_manager_ =
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
participant->get_domain_id()
)
);
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 =
@@ -144,11 +126,7 @@ bool Agent::create(
const eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataReader* datareader) -> void
{
auto graph_manager_ =
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
participant->get_domain_id()
)
);
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 =
@@ -174,11 +152,7 @@ bool Agent::create(
const eprosima::fastdds::dds::DomainParticipant* participant,
const eprosima::fastdds::dds::DataReader* datareader) -> void
{
auto graph_manager_ =
find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t(
participant->get_domain_id()
)
);
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 =
@@ -204,16 +178,14 @@ void Agent::run()
std::shared_ptr<graph_manager::GraphManager> 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;
}else{
return graph_manager_map_.insert(
std::pair<
eprosima::fastdds::dds::DomainId_t,
std::shared_ptr<graph_manager::GraphManager>
>(
std::make_pair(
domain_id,
std::make_shared<graph_manager::GraphManager>(domain_id)
)
micro_ros_agent/src/agent/graph_manager/graph_manager.cpp
+52 -37
View File
@@ -22,11 +22,9 @@ namespace agent {
namespace graph_manager {
GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
// : eprosima::fastrtps::ParticipantListener()
: domain_id_(domain_id)
, graph_changed_(false)
, display_on_change_(false)
, enclave_("/")
, mtx_()
, cv_()
, graphCache_()
@@ -43,12 +41,13 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
eprosima::fastdds::dds::DomainParticipantQos participant_qos =
eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos();
size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave_) + 1;
const char * enclave = "/";
size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1;
participant_qos.user_data().resize(length);
snprintf(reinterpret_cast<char *>(participant_qos.user_data().data_vec().data()),
length, "enclave=%s;", enclave_);
length, "enclave=%s;", enclave);
participant_qos.name(enclave_);
participant_qos.name(enclave);
participant_qos.wire_protocol().builtin.readerHistoryMemoryPolicy =
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
participant_qos.wire_protocol().builtin.writerHistoryMemoryPolicy =
@@ -80,28 +79,26 @@ GraphManager::GraphManager(eprosima::fastdds::dds::DomainId_t domain_id)
eprosima::fastdds::dds::TOPIC_QOS_DEFAULT));
// Create datawriters
eprosima::fastdds::dds::DataWriterQos datawriter_qos =
datawriter_qos_ =
eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT;
datawriter_qos.history().kind =
datawriter_qos_.history().kind =
eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
datawriter_qos.history().depth = 1;
datawriter_qos.endpoint().history_memory_policy =
datawriter_qos_.history().depth = 1;
datawriter_qos_.endpoint().history_memory_policy =
eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
datawriter_qos.publish_mode().kind =
datawriter_qos_.publish_mode().kind =
eprosima::fastdds::dds::PublishModeQosPolicyKind::ASYNCHRONOUS_PUBLISH_MODE;
datawriter_qos.reliability().kind =
datawriter_qos_.reliability().kind =
eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
datawriter_qos.durability().kind =
datawriter_qos_.durability().kind =
eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS;
ros_discovery_datawriter_.reset(
publisher_->create_datawriter(ros_discovery_topic_.get(), datawriter_qos));
datawriter_qos.history().kind =
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(), datawriter_qos));
publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), ros_to_microros_datawriter_qos_));
// Create datareaders
@@ -295,37 +292,55 @@ inline void GraphManager::publish_microros_graph()
}
void GraphManager::add_participant(
const eprosima::fastdds::dds::DomainParticipant* participant)
{
const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos();
this->add_participant(participant->guid(), qos.name().to_string(), enclave_);
}
void GraphManager::add_participant(
const eprosima::fastrtps::rtps::GUID_t& guid,
const std::string& node_name,
const eprosima::fastdds::dds::DomainParticipant* participant,
bool from_microros,
const std::string& enclave)
{
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid);
const eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos();
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid());
graphCache_.add_participant(gid, enclave);
if (node_name != enclave) // Do not add root node
// Do not add root node and
// do not announce non-micro-ROS participants
if (qos.name().to_string() != "/" && from_microros)
{
std::string isolated_node_name, isolated_namespace;
get_name_and_namespace(node_name, isolated_node_name, isolated_namespace);
get_name_and_namespace(qos.name().to_string(), isolated_node_name, isolated_namespace);
rmw_dds_common::msg::ParticipantEntitiesInfo info =
graphCache_.add_node(gid, isolated_node_name, isolated_namespace);
ros_discovery_datawriter_->write(static_cast<void *>(&info));
auto it = micro_ros_graph_datawriters_.find(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_));
it = micro_ros_graph_datawriters_.insert(
std::make_pair(participant, std::move(datawriter))).first;
}
it->second->write(static_cast<void *>(&info));
}
}
void GraphManager::remove_participant(
const eprosima::fastrtps::rtps::GUID_t& guid)
const eprosima::fastdds::dds::DomainParticipant* participant,
bool from_microros)
{
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid);
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", participant->guid());
graphCache_.remove_participant(gid);
if (from_microros)
{
rmw_dds_common::msg::ParticipantEntitiesInfo info;
rmw_dds_common::convert_gid_to_msg(&gid, &info.gid);
auto it = micro_ros_graph_datawriters_.find(participant);
it->second->write(static_cast<void *>(&info));
}
micro_ros_graph_datawriters_.erase(participant);
}
void GraphManager::add_datawriter(
@@ -438,7 +453,9 @@ void GraphManager::associate_entity(
break;
}
}
ros_discovery_datawriter_->write(static_cast<void *>(&info));
auto it = micro_ros_graph_datawriters_.find(participant);
it->second->write(static_cast<void *>(&info));
}
@@ -592,13 +609,13 @@ void GraphManager::ParticipantListener::on_participant_discovery(
const std::string enclave =
std::string(name_found->second.begin(), name_found->second.end());
graphManager_from_->add_participant(participant->guid(), info.info.m_participantName.to_string(), enclave);
graphManager_from_->add_participant(participant, false, enclave);
break;
}
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT:
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT:
{
graphManager_from_->remove_participant(info.info.m_guid);
graphManager_from_->remove_participant(participant, false);
break;
}
default:
@@ -713,7 +730,6 @@ void GraphManager::ParticipantListener::on_subscriber_discovery(
eprosima::fastrtps::rtps::ReaderDiscoveryInfo&& info)
{
process_discovery_info<eprosima::fastrtps::rtps::ReaderDiscoveryInfo>(info);
// graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAREADER);
}
void GraphManager::ParticipantListener::on_publisher_discovery(
@@ -721,7 +737,6 @@ void GraphManager::ParticipantListener::on_publisher_discovery(
eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info)
{
process_discovery_info<eprosima::fastrtps::rtps::WriterDiscoveryInfo>(info);
// graphManager_from_->associate_entity(info.info.guid(), participant, dds::xrce::OBJK_DATAWRITER);
}
GraphManager::DatareaderListener::DatareaderListener(