mirror of
https://github.com/micro-ROS/micro-ROS-Agent.git
synced 2026-09-06 18:05:17 +02:00
Feature/graph manager (#38)
* Initial changes * Working state * Working state * Working state * Working state * Working state * Working state * Added graph listener * Updates * Testing name demangling * Graph manager: reworked * Remove debug trace * Treat errors when freeing rcutils resources * Improve logics for discovering entities * Working state: Micro-ROS graph manager * Fix micro_ros_graph_info typesupport type name * Remove double CMakeLists comment * Remove package.xml commented block * Add missing include * Added CI * Remove GUID_t param from callbacks signature * Add services names and types to micro-ROS graph topic * Differenciate between service server and clients * remove debug traces Co-authored-by: Pablo Garrido <pablogs9@gmail.com>
This commit is contained in:
Jose Antonio Moral
co-authored by
Pablo Garrido
Pablo Garrido
parent
dce3c25a33
commit
23bcea8efb
@@ -0,0 +1,166 @@
|
||||
// Copyright 2020 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef _UROS_AGENT_AGENT_CPP
|
||||
#define _UROS_AGENT_AGENT_CPP
|
||||
|
||||
#include <agent/Agent.hpp>
|
||||
|
||||
namespace uros {
|
||||
namespace agent {
|
||||
|
||||
Agent::Agent()
|
||||
: xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance())
|
||||
, graph_manager_(std::make_unique<graph_manager::GraphManager>())
|
||||
{
|
||||
/**
|
||||
* Add CREATE_PARTICIPANT callback.
|
||||
*/
|
||||
std::function<void (
|
||||
const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
|
||||
([&](
|
||||
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
|
||||
{
|
||||
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<void (
|
||||
const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
|
||||
([&](
|
||||
const eprosima::fastdds::dds::DomainParticipant* participant) -> void
|
||||
{
|
||||
graph_manager_->remove_participant(participant->guid());
|
||||
});
|
||||
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<void (
|
||||
const eprosima::fastdds::dds::DomainParticipant *,
|
||||
const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
|
||||
([&](
|
||||
const eprosima::fastdds::dds::DomainParticipant* participant,
|
||||
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
|
||||
{
|
||||
// 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<void (
|
||||
const eprosima::fastdds::dds::DomainParticipant *,
|
||||
const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
|
||||
([&](
|
||||
const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
|
||||
const eprosima::fastdds::dds::DataWriter* datawriter) -> void
|
||||
{
|
||||
// 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<void (
|
||||
const eprosima::fastdds::dds::DomainParticipant *,
|
||||
const eprosima::fastdds::dds::DataReader*)> on_create_datareader
|
||||
([&](
|
||||
const eprosima::fastdds::dds::DomainParticipant* participant,
|
||||
const eprosima::fastdds::dds::DataReader* datareader) -> void
|
||||
{
|
||||
// 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<void (
|
||||
const eprosima::fastdds::dds::DomainParticipant *,
|
||||
const eprosima::fastdds::dds::DataReader *)> on_delete_datareader
|
||||
([&](
|
||||
const eprosima::fastdds::dds::DomainParticipant* /*participant*/,
|
||||
const eprosima::fastdds::dds::DataReader* datareader) -> void
|
||||
{
|
||||
// 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));
|
||||
}
|
||||
|
||||
bool Agent::create(
|
||||
int argc,
|
||||
char** argv)
|
||||
{
|
||||
return xrce_dds_agent_instance_.create(argc, argv);
|
||||
}
|
||||
|
||||
void Agent::run()
|
||||
{
|
||||
return xrce_dds_agent_instance_.run();
|
||||
}
|
||||
|
||||
} // namespace agent
|
||||
} // namespace uros
|
||||
#endif // _UROS_AGENT_AGENT_CPP
|
||||
@@ -0,0 +1,688 @@
|
||||
// Copyright 2020 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef _UROS_AGENT_GRAPH_MANAGER_CPP
|
||||
#define _UROS_AGENT_GRAPH_MANAGER_CPP
|
||||
|
||||
#include <agent/graph_manager/graph_manager.hpp>
|
||||
|
||||
namespace uros {
|
||||
namespace agent {
|
||||
namespace graph_manager {
|
||||
|
||||
GraphManager::GraphManager()
|
||||
// : eprosima::fastrtps::ParticipantListener()
|
||||
: graph_changed_(false)
|
||||
, display_on_change_(false)
|
||||
, enclave_("/")
|
||||
, mtx_()
|
||||
, cv_()
|
||||
, graphCache_()
|
||||
, participant_listener_(std::make_unique<ParticipantListener>(this))
|
||||
, datareader_listener_(std::make_unique<DatareaderListener>(this))
|
||||
, participant_info_typesupport_(std::make_unique<
|
||||
eprosima::fastdds::dds::TypeSupport>(new graph_manager::ParticipantEntitiesInfoTypeSupport()))
|
||||
, microros_graph_info_typesupport_(std::make_unique<
|
||||
eprosima::fastdds::dds::TypeSupport>(new graph_manager::MicrorosGraphInfoTypeSupport()))
|
||||
{
|
||||
// Create DomainParticipant
|
||||
eprosima::fastdds::dds::DomainId_t domain_id(0);
|
||||
|
||||
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;
|
||||
participant_qos.user_data().resize(length);
|
||||
snprintf(reinterpret_cast<char *>(participant_qos.user_data().data_vec().data()),
|
||||
length, "enclave=%s;", 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 =
|
||||
eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
|
||||
|
||||
participant_.reset(eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->
|
||||
create_participant(domain_id, participant_qos, participant_listener_.get()));
|
||||
|
||||
// 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));
|
||||
|
||||
// Create subscriber
|
||||
subscriber_.reset(participant_->create_subscriber(
|
||||
eprosima::fastdds::dds::SUBSCRIBER_QOS_DEFAULT));
|
||||
|
||||
// Create topics
|
||||
ros_discovery_topic_.reset(participant_->create_topic("ros_discovery_info",
|
||||
participant_info_typesupport_->get_type_name(),
|
||||
eprosima::fastdds::dds::TOPIC_QOS_DEFAULT));
|
||||
|
||||
ros_to_microros_graph_topic_.reset(participant_->create_topic("ros_to_microros_graph",
|
||||
microros_graph_info_typesupport_->get_type_name(),
|
||||
eprosima::fastdds::dds::TOPIC_QOS_DEFAULT));
|
||||
|
||||
// Create datawriters
|
||||
eprosima::fastdds::dds::DataWriterQos datawriter_qos =
|
||||
eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT;
|
||||
|
||||
datawriter_qos.history().kind =
|
||||
eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
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 =
|
||||
eprosima::fastdds::dds::PublishModeQosPolicyKind::ASYNCHRONOUS_PUBLISH_MODE;
|
||||
datawriter_qos.reliability().kind =
|
||||
eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
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::HistoryQosPolicyKind::KEEP_ALL_HISTORY_QOS;
|
||||
ros_to_microros_graph_datawriter_.reset(
|
||||
publisher_->create_datawriter(ros_to_microros_graph_topic_.get(), datawriter_qos));
|
||||
|
||||
// Create datareaders
|
||||
|
||||
eprosima::fastdds::dds::DataReaderQos datareader_qos =
|
||||
eprosima::fastdds::dds::DATAREADER_QOS_DEFAULT;
|
||||
datareader_qos.history().kind =
|
||||
eprosima::fastdds::dds::HistoryQosPolicyKind::KEEP_LAST_HISTORY_QOS;
|
||||
datareader_qos.history().depth = 1;
|
||||
datareader_qos.endpoint().history_memory_policy =
|
||||
eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
|
||||
datareader_qos.reliability().kind =
|
||||
eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS;
|
||||
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()));
|
||||
|
||||
// Set graph cache on change callback function
|
||||
graphCache_.set_on_change_callback([this]()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(this->mtx_);
|
||||
this->graph_changed_ = true;
|
||||
this->cv_.notify_one();
|
||||
});
|
||||
|
||||
microros_graph_publisher_ = std::thread(&GraphManager::publish_microros_graph, this);
|
||||
}
|
||||
|
||||
inline void GraphManager::publish_microros_graph()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mtx_);
|
||||
cv_.wait(lock, [this]()
|
||||
{
|
||||
return this->graph_changed_;
|
||||
});
|
||||
}
|
||||
|
||||
if (display_on_change_)
|
||||
{
|
||||
std::cout << "Updated uros Graph: graph changed" << std::endl;
|
||||
std::cout << graphCache_ << std::endl;
|
||||
}
|
||||
graph_changed_ = false;
|
||||
|
||||
micro_ros_msgs::msg::Graph graph_message;
|
||||
|
||||
rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array();
|
||||
rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array();
|
||||
rcutils_allocator_t allocator = rcutils_get_default_allocator();
|
||||
|
||||
graphCache_.get_node_names(&node_names, &node_namespaces, nullptr, &allocator);
|
||||
|
||||
for (size_t i = 0; i < node_names.size; ++i)
|
||||
{
|
||||
const std::string node_name(node_names.data[i]);
|
||||
const std::string node_namespace(node_namespaces.data[i]);
|
||||
|
||||
micro_ros_msgs::msg::Node node_message;
|
||||
node_message.node_namespace = std::move(node_namespace);
|
||||
node_message.node_name = std::move(node_name);
|
||||
|
||||
// Get publishers info
|
||||
rmw_names_and_types_t writer_names_and_types =
|
||||
rmw_get_zero_initialized_names_and_types();
|
||||
if (RMW_RET_OK != graphCache_.get_writer_names_and_types_by_node(node_name, node_namespace,
|
||||
uros::agent::utils::Demangle::demangle_ros_topic_from_topic,
|
||||
uros::agent::utils::Demangle::demangle_if_ros_type,
|
||||
&allocator, &writer_names_and_types))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < writer_names_and_types.names.size; ++i)
|
||||
{
|
||||
micro_ros_msgs::msg::Entity entity_message;
|
||||
entity_message.entity_type = micro_ros_msgs::msg::Entity::PUBLISHER;
|
||||
entity_message.name = std::move(std::string(writer_names_and_types.names.data[i]));
|
||||
|
||||
for (size_t j = 0; j < writer_names_and_types.types[i].size; ++j)
|
||||
{
|
||||
entity_message.types.emplace_back(writer_names_and_types.types[i].data[j]);
|
||||
}
|
||||
|
||||
node_message.entities.emplace_back(std::move(entity_message));
|
||||
}
|
||||
|
||||
// Get subscribers info
|
||||
rmw_names_and_types_t reader_names_and_types =
|
||||
rmw_get_zero_initialized_names_and_types();
|
||||
if (RMW_RET_OK != graphCache_.get_reader_names_and_types_by_node(node_name, node_namespace,
|
||||
uros::agent::utils::Demangle::demangle_ros_topic_from_topic,
|
||||
uros::agent::utils::Demangle::demangle_if_ros_type,
|
||||
&allocator, &reader_names_and_types))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < reader_names_and_types.names.size; ++i)
|
||||
{
|
||||
micro_ros_msgs::msg::Entity entity_message;
|
||||
entity_message.entity_type = micro_ros_msgs::msg::Entity::SUBSCRIBER;
|
||||
entity_message.name = std::move(std::string(reader_names_and_types.names.data[i]));
|
||||
|
||||
for (size_t j = 0; j < reader_names_and_types.types[i].size; ++j)
|
||||
{
|
||||
entity_message.types.emplace_back(reader_names_and_types.types[i].data[j]);
|
||||
}
|
||||
|
||||
node_message.entities.emplace_back(std::move(entity_message));
|
||||
}
|
||||
|
||||
// Get services
|
||||
//// Get servers
|
||||
rmw_names_and_types_t service_server_names_and_types =
|
||||
rmw_get_zero_initialized_names_and_types();
|
||||
if (RMW_RET_OK != graphCache_.get_names_and_types(
|
||||
uros::agent::utils::Demangle::demangle_service_request_from_topic,
|
||||
uros::agent::utils::Demangle::demangle_service_type_only,
|
||||
&allocator, &service_server_names_and_types))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < service_server_names_and_types.names.size; ++i)
|
||||
{
|
||||
micro_ros_msgs::msg::Entity entity_message;
|
||||
entity_message.entity_type = micro_ros_msgs::msg::Entity::SERVICE_SERVER;
|
||||
entity_message.name = std::move(std::string(service_server_names_and_types.names.data[i]));
|
||||
|
||||
for (size_t j = 0; j < service_server_names_and_types.types[i].size; ++j)
|
||||
{
|
||||
entity_message.types.emplace_back(service_server_names_and_types.types[i].data[j]);
|
||||
}
|
||||
|
||||
node_message.entities.emplace_back(std::move(entity_message));
|
||||
}
|
||||
|
||||
//// Get clients
|
||||
rmw_names_and_types_t service_client_names_and_types =
|
||||
rmw_get_zero_initialized_names_and_types();
|
||||
if (RMW_RET_OK != graphCache_.get_names_and_types(
|
||||
uros::agent::utils::Demangle::demangle_service_reply_from_topic,
|
||||
uros::agent::utils::Demangle::demangle_service_type_only,
|
||||
&allocator, &service_client_names_and_types))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < service_client_names_and_types.names.size; ++i)
|
||||
{
|
||||
micro_ros_msgs::msg::Entity entity_message;
|
||||
entity_message.entity_type = micro_ros_msgs::msg::Entity::SERVICE_CLIENT;
|
||||
entity_message.name = std::move(std::string(service_client_names_and_types.names.data[i]));
|
||||
|
||||
for (size_t j = 0; j < service_client_names_and_types.types[i].size; ++j)
|
||||
{
|
||||
entity_message.types.emplace_back(service_client_names_and_types.types[i].data[j]);
|
||||
}
|
||||
|
||||
node_message.entities.emplace_back(std::move(entity_message));
|
||||
}
|
||||
|
||||
graph_message.nodes.emplace_back(std::move(node_message));
|
||||
|
||||
if (RMW_RET_OK != rmw_names_and_types_fini(&writer_names_and_types) ||
|
||||
RMW_RET_OK != rmw_names_and_types_fini(&reader_names_and_types) ||
|
||||
RMW_RET_OK != rmw_names_and_types_fini(&service_server_names_and_types) ||
|
||||
RMW_RET_OK != rmw_names_and_types_fini(&service_client_names_and_types))
|
||||
{
|
||||
std::cerr << "Problem while freeing resources in Micro-ROS Graph Manager"
|
||||
<< ", file: '" << __FILE__ << "', line: '" << __LINE__ << "'." << std::endl;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
ros_to_microros_graph_datawriter_->write(static_cast<void *>(&graph_message));
|
||||
|
||||
if (RCUTILS_RET_OK != rcutils_string_array_fini(&node_names) ||
|
||||
RCUTILS_RET_OK != rcutils_string_array_fini(&node_namespaces))
|
||||
{
|
||||
std::cerr << "Problem while freeing resources in Micro-ROS Graph Manager"
|
||||
<< ", file: '" << __FILE__ << "', line: '" << __LINE__ << "'." << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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 std::string& enclave)
|
||||
{
|
||||
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid);
|
||||
|
||||
graphCache_.add_participant(gid, enclave);
|
||||
|
||||
if (node_name != enclave) // Do not add root node
|
||||
{
|
||||
rmw_dds_common::msg::ParticipantEntitiesInfo info =
|
||||
graphCache_.add_node(gid, node_name, enclave_);
|
||||
ros_discovery_datawriter_->write(static_cast<void *>(&info));
|
||||
}
|
||||
}
|
||||
|
||||
void GraphManager::remove_participant(
|
||||
const eprosima::fastrtps::rtps::GUID_t& guid)
|
||||
{
|
||||
const rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid("rmw_fastrtps_cpp", guid);
|
||||
graphCache_.remove_participant(gid);
|
||||
}
|
||||
|
||||
void GraphManager::add_datawriter(
|
||||
const eprosima::fastrtps::rtps::GUID_t& datawriter_guid,
|
||||
const eprosima::fastdds::dds::DomainParticipant* participant,
|
||||
const eprosima::fastdds::dds::DataWriter* datawriter)
|
||||
{
|
||||
const std::string& topic_name = datawriter->get_topic()->get_name();
|
||||
const std::string& type_name = datawriter->get_topic()->get_type_name();
|
||||
this->add_datawriter(datawriter_guid, topic_name, type_name,
|
||||
participant->guid(), datawriter->get_qos());
|
||||
}
|
||||
|
||||
void GraphManager::add_datawriter(
|
||||
const eprosima::fastrtps::rtps::GUID_t& datawriter_guid,
|
||||
const std::string& topic_name,
|
||||
const std::string& type_name,
|
||||
const eprosima::fastrtps::rtps::GUID_t& participant_guid,
|
||||
const eprosima::fastdds::dds::DataWriterQos& writer_qos)
|
||||
{
|
||||
const rmw_gid_t datawriter_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", datawriter_guid);
|
||||
const rmw_gid_t participant_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", participant_guid);
|
||||
const rmw_qos_profile_t qos_profile = fastdds_qos_to_rmw_qos(writer_qos);
|
||||
|
||||
graphCache_.add_entity(datawriter_gid, topic_name,
|
||||
type_name, participant_gid, qos_profile, false);
|
||||
}
|
||||
|
||||
void GraphManager::add_datawriter(
|
||||
const eprosima::fastrtps::rtps::GUID_t& datawriter_guid,
|
||||
const std::string& topic_name,
|
||||
const std::string& type_name,
|
||||
const eprosima::fastrtps::rtps::GUID_t& participant_guid,
|
||||
const eprosima::fastdds::dds::WriterQos& writer_qos)
|
||||
{
|
||||
const rmw_gid_t datawriter_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", datawriter_guid);
|
||||
const rmw_gid_t participant_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", participant_guid);
|
||||
rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown;
|
||||
dds_qos_to_rmw_qos(writer_qos, &qos_profile);
|
||||
|
||||
graphCache_.add_entity(datawriter_gid, topic_name,
|
||||
type_name, participant_gid, qos_profile, false);
|
||||
}
|
||||
|
||||
void GraphManager::remove_datawriter(
|
||||
const eprosima::fastrtps::rtps::GUID_t& datawriter_guid)
|
||||
{
|
||||
const rmw_gid_t datawriter_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", datawriter_guid);
|
||||
|
||||
graphCache_.remove_entity(datawriter_gid, false);
|
||||
}
|
||||
|
||||
void GraphManager::add_datareader(
|
||||
const eprosima::fastrtps::rtps::GUID_t& datareader_guid,
|
||||
const eprosima::fastdds::dds::DomainParticipant* participant,
|
||||
const eprosima::fastdds::dds::DataReader* datareader)
|
||||
{
|
||||
const std::string& topic_name = datareader->get_topicdescription()->get_name();
|
||||
const std::string& type_name = datareader->get_topicdescription()->get_type_name();
|
||||
this->add_datareader(datareader_guid, topic_name, type_name,
|
||||
participant->guid(), datareader->get_qos());
|
||||
}
|
||||
|
||||
void GraphManager::add_datareader(
|
||||
const eprosima::fastrtps::rtps::GUID_t& datareader_guid,
|
||||
const std::string& topic_name,
|
||||
const std::string& type_name,
|
||||
const eprosima::fastrtps::rtps::GUID_t& participant_guid,
|
||||
const eprosima::fastdds::dds::DataReaderQos& reader_qos)
|
||||
{
|
||||
const rmw_gid_t datareader_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", datareader_guid);
|
||||
const rmw_gid_t participant_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", participant_guid);
|
||||
const rmw_qos_profile_t qos_profile = fastdds_qos_to_rmw_qos(reader_qos);
|
||||
|
||||
graphCache_.add_entity(datareader_gid, topic_name,
|
||||
type_name, participant_gid, qos_profile, true);
|
||||
}
|
||||
|
||||
void GraphManager::add_datareader(
|
||||
const eprosima::fastrtps::rtps::GUID_t& datareader_guid,
|
||||
const std::string& topic_name,
|
||||
const std::string& type_name,
|
||||
const eprosima::fastrtps::rtps::GUID_t& participant_guid,
|
||||
const eprosima::fastdds::dds::ReaderQos& reader_qos)
|
||||
{
|
||||
const rmw_gid_t datareader_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", datareader_guid);
|
||||
const rmw_gid_t participant_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", participant_guid);
|
||||
rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown;
|
||||
dds_qos_to_rmw_qos(reader_qos, &qos_profile);
|
||||
|
||||
graphCache_.add_entity(datareader_gid, topic_name,
|
||||
type_name, participant_gid, qos_profile, true);
|
||||
}
|
||||
|
||||
void GraphManager::remove_datareader(
|
||||
const eprosima::fastrtps::rtps::GUID_t& datareader_guid)
|
||||
{
|
||||
const rmw_gid_t datareader_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", datareader_guid);
|
||||
|
||||
graphCache_.remove_entity(datareader_gid, true);
|
||||
}
|
||||
|
||||
void GraphManager::associate_entity(
|
||||
const eprosima::fastrtps::rtps::GUID_t& entity_guid,
|
||||
const eprosima::fastdds::dds::DomainParticipant* participant,
|
||||
const dds::xrce::ObjectKind& entity_kind)
|
||||
{
|
||||
const rmw_gid_t entity_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", entity_guid);
|
||||
const rmw_gid_t participant_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
|
||||
"rmw_fastrtps_cpp", participant->guid());
|
||||
|
||||
eprosima::fastdds::dds::DomainParticipantQos qos = participant->get_qos();
|
||||
rmw_dds_common::msg::ParticipantEntitiesInfo info;
|
||||
|
||||
switch (entity_kind)
|
||||
{
|
||||
case dds::xrce::OBJK_DATAWRITER:
|
||||
{
|
||||
info = graphCache_.associate_writer(
|
||||
entity_gid, participant_gid, qos.name().c_str(), enclave_);
|
||||
break;
|
||||
}
|
||||
case dds::xrce::OBJK_DATAREADER:
|
||||
{
|
||||
info = graphCache_.associate_reader(
|
||||
entity_gid, participant_gid, qos.name().c_str(), enclave_);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
ros_discovery_datawriter_->write(static_cast<void *>(&info));
|
||||
}
|
||||
|
||||
|
||||
template <typename FastDDSQos>
|
||||
const rmw_qos_profile_t GraphManager::fastdds_qos_to_rmw_qos(
|
||||
const FastDDSQos& fastdds_qos)
|
||||
{
|
||||
rmw_qos_profile_t rmw_qos = rmw_qos_profile_unknown;
|
||||
switch (fastdds_qos.reliability().kind)
|
||||
{
|
||||
case eprosima::fastdds::dds::ReliabilityQosPolicyKind::BEST_EFFORT_RELIABILITY_QOS:
|
||||
{
|
||||
rmw_qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
|
||||
break;
|
||||
}
|
||||
case eprosima::fastdds::dds::ReliabilityQosPolicyKind::RELIABLE_RELIABILITY_QOS:
|
||||
{
|
||||
rmw_qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
rmw_qos.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
switch (fastdds_qos.durability().kind)
|
||||
{
|
||||
case eprosima::fastdds::dds::DurabilityQosPolicyKind::TRANSIENT_LOCAL_DURABILITY_QOS:
|
||||
{
|
||||
rmw_qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
|
||||
break;
|
||||
}
|
||||
case eprosima::fastdds::dds::DurabilityQosPolicyKind::VOLATILE_DURABILITY_QOS:
|
||||
{
|
||||
rmw_qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
rmw_qos.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
rmw_qos.deadline.sec = fastdds_qos.deadline().period.seconds;
|
||||
rmw_qos.deadline.nsec = fastdds_qos.deadline().period.nanosec;
|
||||
|
||||
rmw_qos.lifespan.sec = fastdds_qos.lifespan().duration.seconds;
|
||||
rmw_qos.lifespan.nsec = fastdds_qos.lifespan().duration.nanosec;
|
||||
|
||||
switch (fastdds_qos.liveliness().kind)
|
||||
{
|
||||
case eprosima::fastdds::dds::LivelinessQosPolicyKind::AUTOMATIC_LIVELINESS_QOS:
|
||||
{
|
||||
rmw_qos.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
|
||||
break;
|
||||
}
|
||||
case eprosima::fastdds::dds::LivelinessQosPolicyKind::MANUAL_BY_TOPIC_LIVELINESS_QOS:
|
||||
{
|
||||
rmw_qos.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
rmw_qos.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
rmw_qos.liveliness_lease_duration.sec = fastdds_qos.liveliness().lease_duration.seconds;
|
||||
rmw_qos.liveliness_lease_duration.nsec = fastdds_qos.liveliness().lease_duration.nanosec;
|
||||
|
||||
return rmw_qos;
|
||||
}
|
||||
|
||||
void GraphManager::update_node_entities_info()
|
||||
{
|
||||
rmw_dds_common::msg::ParticipantEntitiesInfo entities_info;
|
||||
eprosima::fastdds::dds::SampleInfo sample_info;
|
||||
if (ros_discovery_datareader_->take_next_sample(&entities_info, &sample_info) ==
|
||||
eprosima::fastrtps::types::ReturnCode_t::RETCODE_OK)
|
||||
{
|
||||
if (sample_info.instance_state == eprosima::fastdds::dds::InstanceStateKind::ALIVE)
|
||||
{
|
||||
graphCache_.update_participant_entities(entities_info);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
GraphManager::ParticipantListener::ParticipantListener(
|
||||
GraphManager* graph_manager)
|
||||
: eprosima::fastdds::dds::DomainParticipantListener()
|
||||
, graphManager_from_(graph_manager)
|
||||
{
|
||||
}
|
||||
|
||||
void GraphManager::ParticipantListener::on_participant_discovery(
|
||||
eprosima::fastdds::dds::DomainParticipant* participant,
|
||||
eprosima::fastrtps::rtps::ParticipantDiscoveryInfo&& info)
|
||||
{
|
||||
switch (info.status)
|
||||
{
|
||||
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT:
|
||||
{
|
||||
auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData);
|
||||
auto name_found = map.find("enclave");
|
||||
|
||||
if (map.end() == name_found)
|
||||
{
|
||||
return;
|
||||
}
|
||||
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);
|
||||
break;
|
||||
}
|
||||
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT:
|
||||
case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT:
|
||||
{
|
||||
graphManager_from_->remove_participant(info.info.m_guid);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <>
|
||||
void GraphManager::ParticipantListener::process_discovery_info<eprosima::fastrtps::rtps::ReaderDiscoveryInfo>(
|
||||
const eprosima::fastrtps::rtps::ReaderDiscoveryInfo& reader_info)
|
||||
{
|
||||
switch (reader_info.status)
|
||||
{
|
||||
case eprosima::fastrtps::rtps::ReaderDiscoveryInfo::CHANGED_QOS_READER:
|
||||
{
|
||||
return;
|
||||
}
|
||||
case eprosima::fastrtps::rtps::ReaderDiscoveryInfo::DISCOVERED_READER:
|
||||
{
|
||||
const std::string topic_name = reader_info.info.topicName().to_string();
|
||||
const std::string type_name = reader_info.info.typeName().to_string();
|
||||
|
||||
graphManager_from_->add_datareader(reader_info.info.guid(), topic_name, type_name,
|
||||
iHandle2GUID(reader_info.info.RTPSParticipantKey()), reader_info.info.m_qos);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
graphManager_from_->remove_datareader(reader_info.info.guid());
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <>
|
||||
void GraphManager::ParticipantListener::process_discovery_info<eprosima::fastrtps::rtps::WriterDiscoveryInfo>(
|
||||
const eprosima::fastrtps::rtps::WriterDiscoveryInfo& writer_info)
|
||||
{
|
||||
switch (writer_info.status)
|
||||
{
|
||||
case eprosima::fastrtps::rtps::WriterDiscoveryInfo::CHANGED_QOS_WRITER:
|
||||
{
|
||||
return;
|
||||
}
|
||||
case eprosima::fastrtps::rtps::WriterDiscoveryInfo::DISCOVERED_WRITER:
|
||||
{
|
||||
const std::string topic_name = writer_info.info.topicName().to_string();
|
||||
const std::string type_name = writer_info.info.typeName().to_string();
|
||||
|
||||
graphManager_from_->add_datawriter(writer_info.info.guid(), topic_name, type_name,
|
||||
iHandle2GUID(writer_info.info.RTPSParticipantKey()), writer_info.info.m_qos);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
graphManager_from_->remove_datawriter(writer_info.info.guid());
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GraphManager::ParticipantListener::on_subscriber_discovery(
|
||||
eprosima::fastdds::dds::DomainParticipant* /*participant*/,
|
||||
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(
|
||||
eprosima::fastdds::dds::DomainParticipant* /*participant*/,
|
||||
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(
|
||||
GraphManager* graph_manager)
|
||||
: eprosima::fastdds::dds::DataReaderListener()
|
||||
, graphManager_from_(graph_manager)
|
||||
{
|
||||
}
|
||||
|
||||
void GraphManager::DatareaderListener::on_data_available(
|
||||
eprosima::fastdds::dds::DataReader* /*sub*/)
|
||||
{
|
||||
graphManager_from_->update_node_entities_info();
|
||||
}
|
||||
|
||||
} // namespace graph_manager
|
||||
} // namespace agent
|
||||
} // namespace uros
|
||||
|
||||
#endif // _UROS_AGENT_GRAPH_MANAGER_CPP
|
||||
@@ -0,0 +1,211 @@
|
||||
// Copyright 2020 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef UROS_AGENT_GRAPH_PARTICIPANTS_TYPESUPPORT_CPP_
|
||||
#define UROS_AGENT_GRAPH_PARTICIPANTS_TYPESUPPORT_CPP_
|
||||
|
||||
#include <agent/graph_manager/graph_typesupport.hpp>
|
||||
|
||||
namespace uros {
|
||||
namespace agent {
|
||||
namespace graph_manager {
|
||||
|
||||
ParticipantEntitiesInfoTypeSupport::ParticipantEntitiesInfoTypeSupport()
|
||||
: TopicDataType()
|
||||
{
|
||||
type_support_ = rosidl_typesupport_cpp::get_message_type_support_handle<
|
||||
rmw_dds_common::msg::ParticipantEntitiesInfo>();
|
||||
type_support_ = get_message_typesupport_handle(type_support_,
|
||||
"rosidl_typesupport_fastrtps_cpp");
|
||||
callbacks_ = static_cast<const message_type_support_callbacks_t *>(type_support_->data);
|
||||
|
||||
std::ostringstream ss;
|
||||
const std::string message_namespace(callbacks_->message_namespace_);
|
||||
const std::string message_name(callbacks_->message_name_);
|
||||
|
||||
if (!message_namespace.empty())
|
||||
{
|
||||
ss << message_namespace << "::";
|
||||
}
|
||||
ss << "dds_::" << message_name << "_";
|
||||
this->setName(ss.str().c_str());
|
||||
|
||||
bool full_bounded = true;
|
||||
m_typeSize = 4 + callbacks_->max_serialized_size(full_bounded);
|
||||
}
|
||||
|
||||
bool ParticipantEntitiesInfoTypeSupport::serialize(
|
||||
void * data,
|
||||
eprosima::fastrtps::rtps::SerializedPayload_t * payload)
|
||||
{
|
||||
eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char *>(payload->data),
|
||||
payload->max_size);
|
||||
eprosima::fastcdr::Cdr scdr(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
|
||||
eprosima::fastcdr::Cdr::DDS_CDR);
|
||||
|
||||
scdr.serialize_encapsulation();
|
||||
if (callbacks_->cdr_serialize(data, scdr))
|
||||
{
|
||||
payload->encapsulation = (scdr.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS) ?
|
||||
CDR_BE : CDR_LE;
|
||||
payload->length = static_cast<uint32_t>(scdr.getSerializedDataLength());
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool ParticipantEntitiesInfoTypeSupport::deserialize(
|
||||
eprosima::fastrtps::rtps::SerializedPayload_t * payload,
|
||||
void * data)
|
||||
{
|
||||
eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char *>(payload->data),
|
||||
payload->length);
|
||||
eprosima::fastcdr::Cdr dcdr(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
|
||||
eprosima::fastcdr::Cdr::DDS_CDR);
|
||||
|
||||
dcdr.read_encapsulation();
|
||||
return callbacks_->cdr_deserialize(dcdr, data);
|
||||
}
|
||||
|
||||
std::function<uint32_t()> ParticipantEntitiesInfoTypeSupport::getSerializedSizeProvider(
|
||||
void * data)
|
||||
{
|
||||
return [data, this]() -> uint32_t
|
||||
{
|
||||
return static_cast<uint32_t>(4 + callbacks_->get_serialized_size(data));
|
||||
};
|
||||
}
|
||||
|
||||
void * ParticipantEntitiesInfoTypeSupport::createData()
|
||||
{
|
||||
return static_cast<void *>(nullptr);
|
||||
}
|
||||
|
||||
void ParticipantEntitiesInfoTypeSupport::deleteData(
|
||||
void * data)
|
||||
{
|
||||
(void) data;
|
||||
}
|
||||
|
||||
bool ParticipantEntitiesInfoTypeSupport::getKey(
|
||||
void * data,
|
||||
eprosima::fastrtps::rtps::InstanceHandle_t * handle,
|
||||
bool force_md5)
|
||||
{
|
||||
(void) data;
|
||||
(void) handle;
|
||||
(void) force_md5;
|
||||
return m_isGetKeyDefined;
|
||||
}
|
||||
|
||||
|
||||
MicrorosGraphInfoTypeSupport::MicrorosGraphInfoTypeSupport()
|
||||
: TopicDataType()
|
||||
{
|
||||
type_support_ = rosidl_typesupport_cpp::get_message_type_support_handle<
|
||||
micro_ros_msgs::msg::Graph>();
|
||||
type_support_ = get_message_typesupport_handle(type_support_,
|
||||
"rosidl_typesupport_fastrtps_cpp");
|
||||
callbacks_ = static_cast<const message_type_support_callbacks_t *>(type_support_->data);
|
||||
|
||||
std::ostringstream ss;
|
||||
const std::string message_namespace(callbacks_->message_namespace_);
|
||||
const std::string message_name(callbacks_->message_name_);
|
||||
|
||||
if (!message_namespace.empty())
|
||||
{
|
||||
ss << message_namespace << "::";
|
||||
}
|
||||
ss << "dds_::" << message_name << "_";
|
||||
this->setName(ss.str().c_str());
|
||||
|
||||
bool full_bounded = true;
|
||||
m_typeSize = 4 + callbacks_->max_serialized_size(full_bounded);
|
||||
}
|
||||
|
||||
bool MicrorosGraphInfoTypeSupport::serialize(
|
||||
void * data,
|
||||
eprosima::fastrtps::rtps::SerializedPayload_t * payload)
|
||||
{
|
||||
eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char *>(payload->data),
|
||||
payload->max_size);
|
||||
eprosima::fastcdr::Cdr scdr(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
|
||||
eprosima::fastcdr::Cdr::DDS_CDR);
|
||||
|
||||
scdr.serialize_encapsulation();
|
||||
if (callbacks_->cdr_serialize(data, scdr))
|
||||
{
|
||||
payload->encapsulation = (scdr.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS) ?
|
||||
CDR_BE : CDR_LE;
|
||||
payload->length = static_cast<uint32_t>(scdr.getSerializedDataLength());
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool MicrorosGraphInfoTypeSupport::deserialize(
|
||||
eprosima::fastrtps::rtps::SerializedPayload_t * payload,
|
||||
void * data)
|
||||
{
|
||||
eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char *>(payload->data),
|
||||
payload->length);
|
||||
eprosima::fastcdr::Cdr dcdr(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
|
||||
eprosima::fastcdr::Cdr::DDS_CDR);
|
||||
|
||||
dcdr.read_encapsulation();
|
||||
return callbacks_->cdr_deserialize(dcdr, data);
|
||||
}
|
||||
|
||||
std::function<uint32_t()> MicrorosGraphInfoTypeSupport::getSerializedSizeProvider(
|
||||
void * data)
|
||||
{
|
||||
return [data, this]() -> uint32_t
|
||||
{
|
||||
return static_cast<uint32_t>(4 + callbacks_->get_serialized_size(data));
|
||||
};
|
||||
}
|
||||
|
||||
void * MicrorosGraphInfoTypeSupport::createData()
|
||||
{
|
||||
return static_cast<void *>(nullptr);
|
||||
}
|
||||
|
||||
void MicrorosGraphInfoTypeSupport::deleteData(
|
||||
void * data)
|
||||
{
|
||||
(void) data;
|
||||
}
|
||||
|
||||
bool MicrorosGraphInfoTypeSupport::getKey(
|
||||
void * data,
|
||||
eprosima::fastrtps::rtps::InstanceHandle_t * handle,
|
||||
bool force_md5)
|
||||
{
|
||||
(void) data;
|
||||
(void) handle;
|
||||
(void) force_md5;
|
||||
return m_isGetKeyDefined;
|
||||
}
|
||||
|
||||
} // namespace graph_manager
|
||||
} // namespace agent
|
||||
} // namespace uros
|
||||
|
||||
#endif // UROS_AGENT_GRAPH_PARTICIPANTS_TYPESUPPORT_CPP_
|
||||
@@ -0,0 +1,191 @@
|
||||
// Copyright 2020 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef UROS_AGENT_UTILS_DEMANGLE_CPP_
|
||||
#define UROS_AGENT_UTILS_DEMANGLE_CPP_
|
||||
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <rcpputils/find_and_replace.hpp>
|
||||
#include <rcutils/logging_macros.h>
|
||||
#include <rcutils/types.h>
|
||||
|
||||
#include <rmw_fastrtps_shared_cpp/namespace_prefix.hpp>
|
||||
|
||||
#include <agent/utils/demangle.hpp>
|
||||
|
||||
namespace uros {
|
||||
namespace agent {
|
||||
namespace utils {
|
||||
|
||||
std::string Demangle::demangle_if_ros_topic(
|
||||
const std::string& topic_name)
|
||||
{
|
||||
return _strip_ros_prefix_if_exists(topic_name);
|
||||
}
|
||||
|
||||
std::string Demangle::demangle_if_ros_type(
|
||||
const std::string& dds_type_string)
|
||||
{
|
||||
if ('_' != dds_type_string[dds_type_string.size() - 1])
|
||||
{
|
||||
// not a ROS type
|
||||
return dds_type_string;
|
||||
}
|
||||
|
||||
const std::string dds_prefix("dds_::");
|
||||
size_t dds_prefix_pos = dds_type_string.find(dds_prefix);
|
||||
if (std::string::npos == dds_prefix_pos)
|
||||
{
|
||||
// not a ROS type
|
||||
return dds_type_string;
|
||||
}
|
||||
|
||||
std::string type_namespace = dds_type_string.substr(0, dds_prefix_pos);
|
||||
type_namespace = rcpputils::find_and_replace(type_namespace, "::", "/");
|
||||
size_t start = dds_prefix_pos + dds_prefix.size();
|
||||
const std::string type_name =
|
||||
dds_type_string.substr(start, dds_type_string.length() - start - 1);
|
||||
return type_namespace + type_name;
|
||||
}
|
||||
|
||||
std::string Demangle::demangle_ros_topic_from_topic(
|
||||
const std::string& topic_name)
|
||||
{
|
||||
return _resolve_prefix(topic_name, ros_topic_prefix);
|
||||
}
|
||||
|
||||
std::string Demangle::_demangle_service_from_topic(
|
||||
const std::string& prefix,
|
||||
const std::string& topic_name,
|
||||
const std::string& suffix)
|
||||
{
|
||||
const std::string service_name = _resolve_prefix(topic_name, prefix);
|
||||
if (service_name.empty())
|
||||
{
|
||||
return std::string();
|
||||
}
|
||||
|
||||
size_t suffix_position = service_name.rfind(suffix);
|
||||
if (std::string::npos == suffix_position)
|
||||
{
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"rmw_fastrtps_shared_cpp",
|
||||
"service topic has prefix but no suffix; report this: '%s'",
|
||||
topic_name.c_str());
|
||||
return std::string();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (0 != (service_name.length() - suffix_position - suffix.length()))
|
||||
{
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"rmw_fastrtps_shared_cpp",
|
||||
"service topic has service prefix and a suffix"
|
||||
", but not at the end; report this: '%s'",
|
||||
topic_name.c_str());
|
||||
return std::string();
|
||||
}
|
||||
}
|
||||
return service_name.substr(0, suffix_position);
|
||||
}
|
||||
|
||||
std::string Demangle::demangle_service_from_topic(
|
||||
const std::string& topic_name)
|
||||
{
|
||||
const std::string demangled_topic = demangle_service_reply_from_topic(topic_name);
|
||||
|
||||
if (!demangled_topic.empty())
|
||||
{
|
||||
return demangled_topic;
|
||||
}
|
||||
return demangle_service_request_from_topic(topic_name);
|
||||
}
|
||||
|
||||
std::string Demangle::demangle_service_request_from_topic(
|
||||
const std::string& topic_name)
|
||||
{
|
||||
return _demangle_service_from_topic(ros_service_requester_prefix, topic_name, "Request");
|
||||
}
|
||||
|
||||
std::string Demangle::demangle_service_reply_from_topic(
|
||||
const std::string& topic_name)
|
||||
{
|
||||
return _demangle_service_from_topic(ros_service_response_prefix, topic_name, "Reply");
|
||||
}
|
||||
|
||||
std::string Demangle::demangle_service_type_only(
|
||||
const std::string& dds_type_name)
|
||||
{
|
||||
const std::string dds_prefix("dds_::");
|
||||
auto suffixes = {std::string("_Response_"), std::string("_Request_")};
|
||||
size_t dds_prefix_pos = dds_type_name.find(dds_prefix);
|
||||
|
||||
// Perform checks
|
||||
if (std::string::npos == dds_prefix_pos)
|
||||
{
|
||||
// not a ROS service type
|
||||
return std::string();
|
||||
}
|
||||
|
||||
size_t suffix_position = std::string::npos;
|
||||
for (const auto& suffix : suffixes)
|
||||
{
|
||||
suffix_position = dds_type_name.rfind(suffix);
|
||||
if (std::string::npos != suffix_position)
|
||||
{
|
||||
if (0 != (dds_type_name.length() - suffix_position - suffix.length()))
|
||||
{
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"rmw_fastrtps_shared_cpp",
|
||||
"service type contains 'dds_::' and a suffix"
|
||||
", but not at the end; repor this: '%s'",
|
||||
dds_type_name.c_str());
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (std::string::npos == suffix_position)
|
||||
{
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"rmw_fastrtps_shared_cpp",
|
||||
"service type contains 'dds_::' but"
|
||||
" does not have a suffix; report this: '%s'",
|
||||
dds_type_name.c_str());
|
||||
return std::string();
|
||||
}
|
||||
|
||||
// Everything is OK. Reformat it from '[type_namespace::]dds_::<type><suffix>'
|
||||
// to '[type_namespace/]<type>'
|
||||
std::string type_namespace = dds_type_name.substr(0, dds_prefix_pos);
|
||||
type_namespace = rcpputils::find_and_replace(type_namespace, "::", "/");
|
||||
size_t start = dds_prefix_pos + dds_prefix.length();
|
||||
const std::string type_name = dds_type_name.substr(start, suffix_position - start);
|
||||
return type_namespace + type_name;
|
||||
}
|
||||
|
||||
std::string Demangle::identity_demangle(
|
||||
const std::string& name)
|
||||
{
|
||||
return name;
|
||||
}
|
||||
|
||||
} // namespace utils
|
||||
} // namespace agent
|
||||
} // namespace uros
|
||||
|
||||
#endif // UROS_AGENT_UTILS_DEMANGLE_CPP_
|
||||
@@ -12,18 +12,17 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <uxr/agent/AgentInstance.hpp>
|
||||
#include <agent/Agent.hpp>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
eprosima::uxr::AgentInstance& xrce_dds_agent_instance =
|
||||
xrce_dds_agent_instance.getInstance();
|
||||
uros::agent::Agent micro_ros_agent;
|
||||
|
||||
if (!xrce_dds_agent_instance.create(argc, argv))
|
||||
if (!micro_ros_agent.create(argc, argv))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
xrce_dds_agent_instance.run();
|
||||
micro_ros_agent.run();
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user