From 23bcea8efb0c59446c18d701a1f16eccf198dd73 Mon Sep 17 00:00:00 2001 From: Jose Antonio Moral Date: Thu, 26 Nov 2020 07:44:52 +0100 Subject: [PATCH] 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 --- .github/ISSUE_TEMPLATE/bug_report.md | 30 + .github/workflows/ci.yml | 26 + micro_ros_agent/CMakeLists.txt | 65 +- micro_ros_agent/include/agent/Agent.hpp | 49 ++ .../agent/graph_manager/graph_manager.hpp | 322 ++++++++ .../agent/graph_manager/graph_typesupport.hpp | 127 ++++ .../include/agent/utils/demangle.hpp | 125 ++++ micro_ros_agent/package.xml | 17 +- micro_ros_agent/src/agent/Agent.cpp | 166 +++++ .../src/agent/graph_manager/graph_manager.cpp | 688 ++++++++++++++++++ .../agent/graph_manager/graph_typesupport.cpp | 211 ++++++ micro_ros_agent/src/agent/utils/demangle.cpp | 191 +++++ micro_ros_agent/src/main.cpp | 11 +- micro_ros_msgs/CMakeLists.txt | 43 ++ micro_ros_msgs/msg/Entity.msg | 8 + micro_ros_msgs/msg/Graph.msg | 1 + micro_ros_msgs/msg/Node.msg | 3 + micro_ros_msgs/package.xml | 22 + 18 files changed, 2090 insertions(+), 15 deletions(-) create mode 100644 .github/ISSUE_TEMPLATE/bug_report.md create mode 100644 .github/workflows/ci.yml create mode 100644 micro_ros_agent/include/agent/Agent.hpp create mode 100644 micro_ros_agent/include/agent/graph_manager/graph_manager.hpp create mode 100644 micro_ros_agent/include/agent/graph_manager/graph_typesupport.hpp create mode 100644 micro_ros_agent/include/agent/utils/demangle.hpp create mode 100644 micro_ros_agent/src/agent/Agent.cpp create mode 100644 micro_ros_agent/src/agent/graph_manager/graph_manager.cpp create mode 100644 micro_ros_agent/src/agent/graph_manager/graph_typesupport.cpp create mode 100644 micro_ros_agent/src/agent/utils/demangle.cpp create mode 100644 micro_ros_msgs/CMakeLists.txt create mode 100644 micro_ros_msgs/msg/Entity.msg create mode 100644 micro_ros_msgs/msg/Graph.msg create mode 100644 micro_ros_msgs/msg/Node.msg create mode 100644 micro_ros_msgs/package.xml diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000..07f74f4 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,30 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: '' +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behaviour: +1. Clone repo '...' +2. Add these lines '...' +3. Run the script '....' +4. See error +Code or shell commands ready to be copy-pasted are welcome. + +**Expected behaviour** +A clear and concise description of what you expected to happen. + +**System information (please complete the following information):** + - OS: [e.g. WIndows 10, Ubuntu 16.04, ...] + - ROS 2 [e.g. Dashing, Foxy, ...] + - Version [e.g. commit hash, tag, ...] + +**Additional context** +Add any other context about the problem here. diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..106e26e --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,26 @@ +name: CI micro-ROS Agent + +on: + pull_request: + branches: + - '**' + +jobs: + + microros_agent_ci: + runs-on: ubuntu-20.04 + container: ros:foxy + + steps: + - uses: actions/checkout@v2 + with: + path: src/micro_ros_agent + + - name: Download dependencies + run: | + git clone -b foxy https://github.com/eProsima/Micro-XRCE-DDS-Agent src/microxrcedds_agent + + - name: Build + run: | + . /opt/ros/foxy/setup.sh + colcon build \ No newline at end of file diff --git a/micro_ros_agent/CMakeLists.txt b/micro_ros_agent/CMakeLists.txt index 47a650f..b6eee22 100644 --- a/micro_ros_agent/CMakeLists.txt +++ b/micro_ros_agent/CMakeLists.txt @@ -23,19 +23,74 @@ project(micro_ros_agent LANGUAGES CXX) find_package(ament_cmake REQUIRED) find_package(microxrcedds_agent REQUIRED) +find_package(rosidl_cmake REQUIRED) +find_package(fastcdr REQUIRED) +find_package(fastrtps REQUIRED) +find_package(fastrtps_cmake_module REQUIRED) +find_package(rmw_dds_common REQUIRED) +find_package(rmw REQUIRED) +find_package(rcutils REQUIRED) +find_package(rmw_fastrtps_shared_cpp REQUIRED) -add_executable(${PROJECT_NAME} src/main.cpp) +find_package(ament_lint_auto REQUIRED) + +find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) +find_package(rosidl_runtime_cpp REQUIRED) +find_package(rosidl_typesupport_cpp REQUIRED) +find_package(ament_cmake_gtest REQUIRED) + +find_package(micro_ros_msgs REQUIRED) + +add_executable(${PROJECT_NAME} + src/main.cpp + src/agent/Agent.cpp + src/agent/graph_manager/graph_manager.cpp + src/agent/graph_manager/graph_typesupport.cpp + src/agent/utils/demangle.cpp + ) + +target_include_directories(${PROJECT_NAME} + PRIVATE + include + ) + +ament_target_dependencies(${PROJECT_NAME} + rosidl_typesupport_fastrtps_cpp + rosidl_runtime_cpp + rosidl_typesupport_cpp + fastcdr + fastrtps + rmw_dds_common + rmw + rmw_fastrtps_shared_cpp + micro_ros_msgs + ) target_link_libraries(${PROJECT_NAME} + microxrcedds_agent + fastcdr + fastrtps + $<$>:rt> + $<$>:dl> + ) + +target_compile_options(${PROJECT_NAME} PRIVATE - microxrcedds_agent - $<$>:rt> - $<$>:dl> + $<$:-Wall> + $<$:-Wextra> + $<$:-pedantic> ) set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD - 11 + 14 + CXX_STANDARD_REQUIRED + YES + ) + +set_target_properties(${PROJECT_NAME} PROPERTIES + CXX_STANDARD + 14 CXX_STANDARD_REQUIRED YES ) diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp new file mode 100644 index 0000000..ca351b5 --- /dev/null +++ b/micro_ros_agent/include/agent/Agent.hpp @@ -0,0 +1,49 @@ +// 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_HPP +#define _UROS_AGENT_AGENT_HPP + +#include +#include +#include + +#include +// TODO(jamoralp): class Documentation +namespace uros { +namespace agent { + +class Agent +{ +public: + + Agent(); + + ~Agent() = default; + + bool create( + int argc, + char** argv); + + void run(); + +private: + + eprosima::uxr::AgentInstance& xrce_dds_agent_instance_; + std::unique_ptr graph_manager_; +}; + +} // namespace agent +} // namespace uros +#endif // _UROS_AGENT_AGENT_HPP \ No newline at end of file diff --git a/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp new file mode 100644 index 0000000..ca0e23f --- /dev/null +++ b/micro_ros_agent/include/agent/graph_manager/graph_manager.hpp @@ -0,0 +1,322 @@ +// 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. + +#include "fastrtps/Domain.h" +#include +#include "fastrtps/attributes/ParticipantAttributes.h" +#include "fastrtps/participant/Participant.h" +#include "fastrtps/participant/ParticipantListener.h" +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/publisher/Publisher.h" +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rmw/types.h" +#include "rmw/names_and_types.h" +#include "rmw/impl/cpp/key_value.hpp" +#include "rmw_dds_common/graph_cache.hpp" +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" + +#include "rcutils/types.h" +#include "rcutils/types/string_array.h" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" + +#include "rmw_dds_common/msg/participant_entities_info.hpp" +#include "micro_ros_msgs/msg/graph.hpp" +#include "micro_ros_msgs/msg/node.hpp" +#include "micro_ros_msgs/msg/entity.hpp" + +#include + +#include +#include + +#ifndef _UROS_AGENT_GRAPH_MANAGER_HPP +#define _UROS_AGENT_GRAPH_MANAGER_HPP + +namespace uros { +namespace agent { +namespace graph_manager { + +/** + * @brief Class that keeps track of the existing entities in the ROS 2 world, + * both coming from micro-ROS or from external ROS 2 applications. + */ +class GraphManager +{ +public: + /** + * @brief Default constructor. + */ + GraphManager(); + + /** + * @brief Default destructor. + */ + ~GraphManager() = default; + + /** + * @brief Implementation of the notification logic that updates the micro-ROS graph. + */ + void publish_microros_graph(); + + /** + * @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 enclave ROS 2 enclave. + */ + void add_participant( + const eprosima::fastrtps::rtps::GUID_t& guid, + const std::string& node_name, + const std::string& enclave); + + /** + * @brief Removes a DDS participant from the graph tree. + * @param guid rtps::GUID_t of the participant to be removed. + */ + void remove_participant( + const eprosima::fastrtps::rtps::GUID_t& guid); + + /** + * @brief Adds a DDS datawriter to the graph tree. + * @param datawriter_guid rtps::GUID_t of the datawriter to be added. + * @param participant Pointer to the participant which owns this datawriter. + * @param datawriter Pointer to the datawriter to be added. + */ + void add_datawriter( + const eprosima::fastrtps::rtps::GUID_t& datawriter_guid, + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataWriter* datawriter); + + /** + * @brief Adds a DDS datawriter to the graph tree. + * @param datawriter_guid rtps::GUID_t of the datawriter to be added. + * @param topic_name Name of the topic to which the datawriter sends information to. + * @param type_name Type name of the sent topic. + * @param participant_guid rtps::GUID_t of the participant which owns this datawriter. + * @param writer_qos QOS of the datawriter to be included into the graph tree. + */ + void 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); + + /** + * @brief Adds a DDS datawriter to the graph tree. + * @param datawriter_guid rtps::GUID_t of the datawriter to be added. + * @param topic_name Name of the topic to which the datawriter sends information to. + * @param type_name Type name of the sent topic. + * @param participant_guid rtps::GUID_t of the participant which owns this datawriter. + * @param writer_qos QOS of the datawriter to be included into the graph tree. + */ + void 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); + + /** + * @brief Removes a DDS datawriter from the graph tree. + * @param datawriter_guid rtps::GUID_t of the datawriter to be removed. + */ + void remove_datawriter( + const eprosima::fastrtps::rtps::GUID_t& datawriter_guid); + + /** + * @brief Adds a DDS datareader to the graph tree. + * @param datareader_guid rtps::GUID_t of the datareader to be added. + * @param participant Pointer to the participant which owns this datareader. + * @param datareader Pointer to the datareader to be added. + */ + void add_datareader( + const eprosima::fastrtps::rtps::GUID_t& datareader_guid, + const eprosima::fastdds::dds::DomainParticipant* participant, + const eprosima::fastdds::dds::DataReader* datareader); + + /** + * @brief Adds a DDS datareader to the graph tree. + * @param datareader_guid rtps::GUID_t of the datareader to be added. + * @param topic_name Name of the topic to which the datareader sends information to. + * @param type_name Type name of the sent topic. + * @param participant_guid rtps::GUID_t of the participant which owns this datareader. + * @param writer_qos QOS of the datareader to be included into the graph tree. + */ + void 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); + + /** + * @brief Adds a DDS datareader to the graph tree. + * @param datareader_guid rtps::GUID_t of the datareader to be added. + * @param topic_name Name of the topic to which the datareader sends information to. + * @param type_name Type name of the sent topic. + * @param participant_guid rtps::GUID_t of the participant which owns this datareader. + * @param writer_qos QOS of the datareader to be included into the graph tree. + */ + void 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); + + /** + * @brief Removes a DDS datareader from the graph tree. + * @param datareader_guid rtps::GUID_t of the datareader to be removed. + */ + void remove_datareader( + const eprosima::fastrtps::rtps::GUID_t& datareader_guid); + + /** + * @brief Associates a certain DDS entity with a provided participant. + * @param guid rtps::GUID_t identifier of the entity. + * @param participant Participant to be associated with. + * @param entity_kind Kind of the DDS entity. + */ + void associate_entity( + const eprosima::fastrtps::rtps::GUID_t& entity_guid, + const eprosima::fastdds::dds::DomainParticipant* participant, + const dds::xrce::ObjectKind& entity_kind); + +private: + + /** + * @brief Implementation of FastDDS' DomainParticipantListener abstract class. + */ + class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantListener + { + public: + + /** + * @brief Constructor. + * @param graph_manager Pointer to the GraphManager object which owns this ParticipantListener. + */ + ParticipantListener( + GraphManager* graph_manager); + private: + + template + void process_discovery_info( + const Info& proxyData); + + void on_participant_discovery( + eprosima::fastdds::dds::DomainParticipant* participant, + eprosima::fastrtps::rtps::ParticipantDiscoveryInfo&& info) override; + + void on_subscriber_discovery( + eprosima::fastdds::dds::DomainParticipant* /*participant*/, + eprosima::fastrtps::rtps::ReaderDiscoveryInfo&& info) override; + + void on_publisher_discovery( + eprosima::fastdds::dds::DomainParticipant* /*participant*/, + eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info) override; + + GraphManager* graphManager_from_; + }; + + /** + * @brief Implementation of FastDDS' DomainReaderListener abstract class. + */ + class DatareaderListener : public eprosima::fastdds::dds::DataReaderListener + { + public: + + /** + * @brief Constructor. + * @param graph_manager Pointer to the GraphManager object which owns this DataReaderListener. + */ + DatareaderListener( + GraphManager* graph_manager); + + private: + + void on_data_available( + eprosima::fastdds::dds::DataReader* /*sub*/) override; + + GraphManager* graphManager_from_; + }; + + /** + * @brief Convert FastDDS QOS object instance to RMW instance. + * @param fastdds_qos QOS instance to be converted. + * @returns RMW object representation of the given FastDDS QOS. + */ + template + const rmw_qos_profile_t fastdds_qos_to_rmw_qos( + const FastDDSQos& fastdds_qos); + + /** + * @brief Update micro-ROS graph information upon new data + * received in the 'ros_discovery_info' topic. + */ + void update_node_entities_info(); + + bool graph_changed_; + bool display_on_change_; + const char * enclave_; + std::thread microros_graph_publisher_; + std::mutex mtx_; + std::condition_variable cv_; + + rmw_dds_common::GraphCache graphCache_; + std::unique_ptr participant_listener_; + std::unique_ptr datareader_listener_; + + 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_discovery_datawriter_; + std::unique_ptr ros_to_microros_graph_datawriter_; + std::unique_ptr ros_discovery_datareader_; +}; + +} // namespace graph_manager +} // namespace agent +} // namespace uros + +#endif // _UROS_AGENT_GRAPH_MANAGER_HPP \ No newline at end of file diff --git a/micro_ros_agent/include/agent/graph_manager/graph_typesupport.hpp b/micro_ros_agent/include/agent/graph_manager/graph_typesupport.hpp new file mode 100644 index 0000000..c88d854 --- /dev/null +++ b/micro_ros_agent/include/agent/graph_manager/graph_typesupport.hpp @@ -0,0 +1,127 @@ +// 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_HPP_ +#define UROS_AGENT_GRAPH_PARTICIPANTS_TYPESUPPORT_HPP_ + +#include "fastrtps/Domain.h" +#include +#include "fastrtps/attributes/ParticipantAttributes.h" +#include "fastrtps/participant/Participant.h" +#include "fastrtps/participant/ParticipantListener.h" +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/publisher/Publisher.h" +#include +#include + +#include +#include + +#include "rmw/types.h" +#include "rmw/impl/cpp/key_value.hpp" +#include "rmw_dds_common/graph_cache.hpp" +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" + +#include "rcutils/types.h" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" + +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "micro_ros_msgs/msg/graph.hpp" + +namespace uros { +namespace agent { +namespace graph_manager { + +/** + * @brief Implementation of virtual class eprosima::fastdds::dds::TopicDataType. + * Is used to gather and send information about the entities present within a DDS domain, + * in the Agent's context. + */ +class ParticipantEntitiesInfoTypeSupport : public eprosima::fastdds::dds::TopicDataType +{ +public: + + ParticipantEntitiesInfoTypeSupport(); + + virtual bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override; + + virtual bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + virtual std::function getSerializedSizeProvider( + void* data) override; + + virtual void* createData() override; + + virtual void deleteData(void* data) override; + + virtual bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* handle, + bool force_md5) override; + +private: + + const message_type_support_callbacks_t* callbacks_; + const rosidl_message_type_support_t* type_support_; +}; + +/** + * @brief Implementation of virtual class eprosima::fastdds::dds::TopicDataType. + * Is used to send graph information to Micro-ROS. + */ +class MicrorosGraphInfoTypeSupport : public eprosima::fastdds::dds::TopicDataType +{ +public: + + MicrorosGraphInfoTypeSupport(); + + virtual bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override; + + virtual bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + virtual std::function getSerializedSizeProvider( + void* data) override; + + virtual void* createData() override; + + virtual void deleteData(void* data) override; + + virtual bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* handle, + bool force_md5) override; + +private: + + const message_type_support_callbacks_t* callbacks_; + const rosidl_message_type_support_t* type_support_; +}; + +} // namespace graph_manager +} // namespace agent +} // namespace uros + +#endif // UROS_AGENT_GRAPH_PARTICIPANTS_TYPESUPPORT_HPP_ \ No newline at end of file diff --git a/micro_ros_agent/include/agent/utils/demangle.hpp b/micro_ros_agent/include/agent/utils/demangle.hpp new file mode 100644 index 0000000..8562215 --- /dev/null +++ b/micro_ros_agent/include/agent/utils/demangle.hpp @@ -0,0 +1,125 @@ +// 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_HPP_ +#define UROS_AGENT_UTILS_DEMANGLE_HPP_ + +#include + +namespace uros { +namespace agent { +namespace utils { + +class Demangle +{ +private: + /** + * @brief Default constructor. Creating instances of this class is not allowed. + */ + Demangle() = default; + + /** + * @brief Default destructor. + */ + ~Demangle() = default; + + /** + * @brief Demangle service name for a given topic, prefix and service suffix, + * if the topic is part of a service; otherwise, return blank. + * @param prefix The ROS service prefix. + * @param topic_name Topic to be demangled. + * @param suffix The ROS service suffix. + * @returns The demangled service name. + */ + static std::string _demangle_service_from_topic( + const std::string& prefix, + const std::string& topic_name, + const std::string& suffix); + +public: + /** + * @brief Demangle if passed topic is a ROS topic; otherwise, keep it intact. + * @param topic_name Topic to be demangled. + * @returns The demangled topic. + */ + static std::string demangle_if_ros_topic( + const std::string& topic_name); + + /** + * @brief Demangle if passed type is a ROS type; otherwise, keep it intact. + * @param dds_type_string Type to be demangled. + * @returns The demangled type. + */ + static std::string demangle_if_ros_type( + const std::string& dds_type_string); + + /** + * @brief Demangle topic name for a given topic if it is part of one; + * otherwise, return empty. + * @param topic_name Topic to be demangled. + * @returns The demangled topic name. + */ + static std::string demangle_ros_topic_from_topic( + const std::string& topic_name); + + /** + * @brief Demangle the service name for a given topic if it is part of a service; + * otherwise, return empty. + * @param topic_name Topic to be demangled. + * @returns The demangled service name. + */ + + static std::string demangle_service_from_topic( + const std::string& topic_name); + + /** + * @brief Demangle the service name for a given topic if it is part + * of a service request; otherwise, return empty. + * @param topic_name Topic to be demangled. + * @returns The demangled service request name. + */ + static std::string demangle_service_request_from_topic( + const std::string& topic_name); + + /** + * @brief Demangle the service name for a given topic if it is part + * of a service reply; otherwise, return empty. + * @param topic_name Topic to be demangled. + * @returns The demangled service reply name. + */ + static std::string demangle_service_reply_from_topic( + const std::string& topic_name); + + /** + * @brief Demangle the service type name if it is a ROS srv type; otherwise, return empty. + * @param dds_type_name Type to be demangled. + * @returns The demangled service type. + */ + static std::string demangle_service_type_only( + const std::string& dds_type_name); + + /** + * @brief Generic demangle function, used when ROS names are not mangled. + * @param name Generic name to be demangled. + * @returns The demangled name. + */ + static std::string identity_demangle( + const std::string& name); +}; + +} // namespace utils +} // namespace agent +} // namespace uros + +#endif // UROS_AGENT_UTILS_DEMANGLE_HPP_ \ No newline at end of file diff --git a/micro_ros_agent/package.xml b/micro_ros_agent/package.xml index 81e2607..fbdd317 100644 --- a/micro_ros_agent/package.xml +++ b/micro_ros_agent/package.xml @@ -8,14 +8,23 @@ Apache License 2.0 ament_cmake - ament_cmake - microxrcedds_agent - rosidl_cmake - + rosidl_default_generators + ament_lint_auto ament_lint_common + microxrcedds_agent + rmw + rcutils + rmw_fastrtps_shared_cpp + rmw_dds_common + micro_ros_msgs + + rosidl_typesupport_fastrtps_cpp + ament_cmake_gtest + builtin_interfaces + ament_cmake diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp new file mode 100644 index 0000000..8a4261b --- /dev/null +++ b/micro_ros_agent/src/agent/Agent.cpp @@ -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 + +namespace uros { +namespace agent { + +Agent::Agent() + : xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance()) + , graph_manager_(std::make_unique()) +{ + /** + * Add CREATE_PARTICIPANT callback. + */ + std::function 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 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 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 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 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 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 \ No newline at end of file diff --git a/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp b/micro_ros_agent/src/agent/graph_manager/graph_manager.cpp new file mode 100644 index 0000000..8432f6c --- /dev/null +++ b/micro_ros_agent/src/agent/graph_manager/graph_manager.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 + +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(this)) + , datareader_listener_(std::make_unique(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(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 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 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(&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(&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(&info)); +} + + +template +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( + 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( + 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(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(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 \ No newline at end of file diff --git a/micro_ros_agent/src/agent/graph_manager/graph_typesupport.cpp b/micro_ros_agent/src/agent/graph_manager/graph_typesupport.cpp new file mode 100644 index 0000000..0f5d016 --- /dev/null +++ b/micro_ros_agent/src/agent/graph_manager/graph_typesupport.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 + +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(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(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(scdr.getSerializedDataLength()); + return true; + } + else + { + return false; + } +} + +bool ParticipantEntitiesInfoTypeSupport::deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t * payload, + void * data) +{ + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(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 ParticipantEntitiesInfoTypeSupport::getSerializedSizeProvider( + void * data) +{ + return [data, this]() -> uint32_t + { + return static_cast(4 + callbacks_->get_serialized_size(data)); + }; +} + +void * ParticipantEntitiesInfoTypeSupport::createData() +{ + return static_cast(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(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(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(scdr.getSerializedDataLength()); + return true; + } + else + { + return false; + } +} + +bool MicrorosGraphInfoTypeSupport::deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t * payload, + void * data) +{ + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(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 MicrorosGraphInfoTypeSupport::getSerializedSizeProvider( + void * data) +{ + return [data, this]() -> uint32_t + { + return static_cast(4 + callbacks_->get_serialized_size(data)); + }; +} + +void * MicrorosGraphInfoTypeSupport::createData() +{ + return static_cast(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_ \ No newline at end of file diff --git a/micro_ros_agent/src/agent/utils/demangle.cpp b/micro_ros_agent/src/agent/utils/demangle.cpp new file mode 100644 index 0000000..de78aef --- /dev/null +++ b/micro_ros_agent/src/agent/utils/demangle.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 +#include +#include + +#include +#include +#include + +#include + +#include + +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_::' + // to '[type_namespace/]' + 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_ \ No newline at end of file diff --git a/micro_ros_agent/src/main.cpp b/micro_ros_agent/src/main.cpp index f255fba..2e7e88e 100644 --- a/micro_ros_agent/src/main.cpp +++ b/micro_ros_agent/src/main.cpp @@ -12,18 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include 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; -} +} \ No newline at end of file diff --git a/micro_ros_msgs/CMakeLists.txt b/micro_ros_msgs/CMakeLists.txt new file mode 100644 index 0000000..7d087be --- /dev/null +++ b/micro_ros_msgs/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(micro_ros_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/Graph.msg" + "msg/Node.msg" + "msg/Entity.msg" +) + +ament_package() diff --git a/micro_ros_msgs/msg/Entity.msg b/micro_ros_msgs/msg/Entity.msg new file mode 100644 index 0000000..8f2209b --- /dev/null +++ b/micro_ros_msgs/msg/Entity.msg @@ -0,0 +1,8 @@ +byte PUBLISHER=0 +byte SUBSCRIBER=1 +byte SERVICE_SERVER=2 +byte SERVICE_CLIENT=3 + +byte entity_type +string<=256 name +string<=256[] types diff --git a/micro_ros_msgs/msg/Graph.msg b/micro_ros_msgs/msg/Graph.msg new file mode 100644 index 0000000..85410ba --- /dev/null +++ b/micro_ros_msgs/msg/Graph.msg @@ -0,0 +1 @@ +Node[] nodes diff --git a/micro_ros_msgs/msg/Node.msg b/micro_ros_msgs/msg/Node.msg new file mode 100644 index 0000000..b881a68 --- /dev/null +++ b/micro_ros_msgs/msg/Node.msg @@ -0,0 +1,3 @@ +string<=256 node_namespace +string<=256 node_name +Entity[] entities diff --git a/micro_ros_msgs/package.xml b/micro_ros_msgs/package.xml new file mode 100644 index 0000000..aaab209 --- /dev/null +++ b/micro_ros_msgs/package.xml @@ -0,0 +1,22 @@ + + + + micro_ros_msgs + 0.0.1 + TODO: Package description + Pablo Garrido + Apache License 2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +