mirror of
https://github.com/micro-ROS/micro-ROS-Agent.git
synced 2026-09-06 18:05:17 +02:00
Compare commits
1
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Pablo Garrido
|
40c5ea4d45 |
@@ -1,30 +0,0 @@
|
||||
---
|
||||
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.
|
||||
@@ -1,32 +0,0 @@
|
||||
name: CI micro-ROS Agent
|
||||
|
||||
on:
|
||||
pull_request:
|
||||
branches:
|
||||
- '**'
|
||||
|
||||
jobs:
|
||||
|
||||
microros_agent_ci:
|
||||
runs-on: ubuntu-20.04
|
||||
container: microros/base:foxy
|
||||
|
||||
steps:
|
||||
- name: Download dependencies
|
||||
run: |
|
||||
cd /uros_ws
|
||||
. /opt/ros/foxy/setup.sh
|
||||
. install/local_setup.sh
|
||||
rosdep update
|
||||
ros2 run micro_ros_setup create_agent_ws.sh
|
||||
|
||||
- uses: actions/checkout@v2
|
||||
with:
|
||||
path: src/uros/micro-ROS-Agent
|
||||
|
||||
- name: Build
|
||||
run: |
|
||||
cd /uros_ws
|
||||
. /opt/ros/foxy/setup.sh
|
||||
. install/local_setup.sh
|
||||
ros2 run micro_ros_setup build_agent.sh
|
||||
@@ -1,4 +0,0 @@
|
||||
Third Party Licenses
|
||||
====================
|
||||
|
||||
This repository does not directly contain 3rd party source code.
|
||||
-107
@@ -1,107 +0,0 @@
|
||||
# Contributing
|
||||
|
||||
Want to contribute? Great! You can do so through the standard GitHub pull
|
||||
request model. For large contributions we do encourage you to file a ticket in
|
||||
the GitHub issues tracking system prior to any code development to coordinate
|
||||
with the system_modes development team early in the process. Coordinating up
|
||||
front helps to avoid frustration later on.
|
||||
|
||||
Your contribution must be licensed under the Apache-2.0 license, the license
|
||||
used by this project.
|
||||
|
||||
## Add / retain copyright notices
|
||||
|
||||
Include a copyright notice and license in each new file to be contributed,
|
||||
consistent with the style used by this project. If your contribution contains
|
||||
code under the copyright of a third party, document its origin, license, and
|
||||
copyright holders.
|
||||
|
||||
## Sign your work
|
||||
|
||||
This project tracks patch provenance and licensing using a modified Developer
|
||||
Certificate of Origin (DCO; from [OSDL][DCO]) and Signed-off-by tags initially
|
||||
developed by the Linux kernel project.
|
||||
|
||||
```
|
||||
system_modes Developer's Certificate of Origin. Version 1.0
|
||||
|
||||
By making a contribution to this project, I certify that:
|
||||
|
||||
(a) The contribution was created in whole or in part by me and I
|
||||
have the right to submit it under the "Apache License, Version 2.0"
|
||||
("Apache-2.0"); or
|
||||
|
||||
(b) The contribution is based upon previous work that is covered by
|
||||
an appropriate open source license and I have the right under
|
||||
that license to submit that work with modifications, whether
|
||||
created in whole or in part by me, under the Apache-2.0 license;
|
||||
or
|
||||
|
||||
(c) The contribution was provided directly to me by some other
|
||||
person who certified (a) or (b) and I have not modified it.
|
||||
|
||||
(d) I understand and agree that this project and the contribution
|
||||
are public and that a record of the contribution (including all
|
||||
metadata and personal information I submit with it, including my
|
||||
sign-off) is maintained indefinitely and may be redistributed
|
||||
consistent with this project and the requirements of the Apache-2.0
|
||||
license or any open source license(s) involved, where they are
|
||||
relevant.
|
||||
|
||||
(e) I am granting the contribution to this project under the terms of
|
||||
Apache-2.0.
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
```
|
||||
|
||||
With the sign-off in a commit message you certify that you authored the patch
|
||||
or otherwise have the right to submit it under an open source license. The
|
||||
procedure is simple: To certify above system_modes Developer's Certificate of
|
||||
Origin 1.0 for your contribution just append a line
|
||||
|
||||
Signed-off-by: Random J Developer <random@developer.example.org>
|
||||
|
||||
to every commit message using your real name or your pseudonym and a valid
|
||||
email address.
|
||||
|
||||
If you have set your `user.name` and `user.email` git configs you can
|
||||
automatically sign the commit by running the git-commit command with the `-s`
|
||||
option. There may be multiple sign-offs if more than one developer was
|
||||
involved in authoring the contribution.
|
||||
|
||||
For a more detailed description of this procedure, please see
|
||||
[SubmittingPatches][] which was extracted from the Linux kernel project, and
|
||||
which is stored in an external repository.
|
||||
|
||||
### Individual vs. Corporate Contributors
|
||||
|
||||
Often employers or academic institution have ownership over code that is
|
||||
written in certain circumstances, so please do due diligence to ensure that
|
||||
you have the right to submit the code.
|
||||
|
||||
If you are a developer who is authorized to contribute to system_modes on
|
||||
behalf of your employer, then please use your corporate email address in the
|
||||
Signed-off-by tag. Otherwise please use a personal email address.
|
||||
|
||||
## Maintain Copyright holder / Contributor list
|
||||
|
||||
Each contributor is responsible for identifying themselves in the
|
||||
[NOTICE](NOTICE) file, the project's list of copyright holders and authors.
|
||||
Please add the respective information corresponding to the Signed-off-by tag
|
||||
as part of your first pull request.
|
||||
|
||||
If you are a developer who is authorized to contribute to system_modes on
|
||||
behalf of your employer, then add your company / organization to the list of
|
||||
copyright holders in the [NOTICE](NOTICE) file. As author of a corporate
|
||||
contribution you can also add your name and corporate email address as in the
|
||||
Signed-off-by tag.
|
||||
|
||||
If your contribution is covered by this project's DCO's clause "(c) The
|
||||
contribution was provided directly to me by some other person who certified
|
||||
(a) or (b) and I have not modified it", please add the appropriate copyright
|
||||
holder(s) to the [NOTICE](NOTICE) file as part of your contribution.
|
||||
|
||||
|
||||
[DCO]: http://web.archive.org/web/20070306195036/http://osdlab.org/newsroom/press_releases/2004/2004_05_24_dco.html
|
||||
|
||||
[SubmittingPatches]: https://github.com/wking/signed-off-by/blob/7d71be37194df05c349157a2161c7534feaf86a4/Documentation/SubmittingPatches
|
||||
@@ -1,27 +0,0 @@
|
||||
|
||||
# This is the official list of copyright holders and authors.
|
||||
#
|
||||
# Often employers or academic institutions have ownership over code that is
|
||||
# written in certain circumstances, so please do due diligence to ensure that
|
||||
# you have the right to submit the code.
|
||||
#
|
||||
# When adding J Random Contributor's name to this file, either J's name on its
|
||||
# own or J's name associated with J's organization's name should be added,
|
||||
# depending on whether J's employer (or academic institution) has ownership
|
||||
# over code that is written for this project.
|
||||
#
|
||||
# How to add names to this file:
|
||||
# Individual's name <submission email address>.
|
||||
#
|
||||
# If Individual's organization is copyright holder of her contributions add the
|
||||
# organization's name, optionally also the contributor's name:
|
||||
#
|
||||
# Organization's name
|
||||
# Individual's name <submission corporate email address>
|
||||
#
|
||||
# Please keep the list sorted.
|
||||
|
||||
eProsima
|
||||
Jose Antonio Moral <joseantoniomoralparras@eprosima.com>
|
||||
Pablo Garrido <pablogarrido@eprosima.com>
|
||||
|
||||
@@ -1,57 +1,7 @@
|
||||
# micro-ROS Agent
|
||||
# micro-ROS-Agent
|
||||
|
||||
[](https://github.com/microROS/micro-ROS-Agent)
|
||||
[](https://github.com/microROS/micro-ROS-Agent/releases)
|
||||
|
||||
ROS 2 package using Micro XRCE-DDS Agent.
|
||||
|
||||
## Overview
|
||||
|
||||
This repository contains the Micro-ROS Agent package.
|
||||
Micro-ROS Agent is a ROS 2 node that wraps the Micro XRCE-DDS Agent.
|
||||
For further information about Micro XRCE-DDS Agent click [here](https://github.com/eProsima/Micro-XRCE-DDS-Agent)
|
||||
This package is a part of the Micro-ROS project stack.
|
||||
For more information about Micro-ROS project click [here](https://microros.github.io/micro-ROS/).
|
||||
|
||||
The node acts as a server between DDS Network and Micro-ROS nodes inside MCU.
|
||||
It receives and send messages from Micro-ROS nodes, and keep track of the Micro-ROS nodes exposing them to the ROS 2 network.
|
||||
The node interacts with DDS Global Data Space on behalf of the Micro-ROS nodes.
|
||||
|
||||
## Package features
|
||||
|
||||
### XML generation
|
||||
|
||||
During the build process, the package looks for all ROS 2 messages to generate an initial list of XML profiles.
|
||||
These profiles can are referenced in the Agent-Client communication to avoid sending the full XML content.
|
||||
This reference mechanism can be switched on and off from the Micro XRCE-DDS middleware layer.
|
||||
|
||||
### Agent-Client communication mechanism
|
||||
|
||||
Communication between the Micro-ROS Agent and the Micro-ROS nodes supports two types of transport:
|
||||
|
||||
- UDP and TCP over IPv4 and IPv6.
|
||||
- Serial Port transports.
|
||||
|
||||
All available configurations are supported directly by the Micro XRCE-DDS agent.
|
||||
|
||||
## Purpose of the Project
|
||||
|
||||
This software is not ready for production use. It has neither been developed nor
|
||||
tested for a specific use case. However, the license conditions of the
|
||||
applicable Open Source licenses allow you to adapt the software to your needs.
|
||||
Before using it in a safety relevant setting, make sure that the software
|
||||
fulfills your requirements and adjust it according to any applicable safety
|
||||
standards, e.g., ISO 26262.
|
||||
|
||||
## License
|
||||
|
||||
This repository is open-sourced under the Apache-2.0 license. See the [LICENSE](LICENSE) file for details.
|
||||
|
||||
For a list of other open-source components included in this repository,
|
||||
see the file [3rd-party-licenses.txt](3rd-party-licenses.txt).
|
||||
|
||||
## Known Issues/Limitations
|
||||
|
||||
Please notice the following issues/limitations:
|
||||
|
||||
* There is an unknown issue when dealing with serial ports shared with the micro-ROS agent running inside a Docker. Sometimes it works with a remarkable packet loss.
|
||||
|
||||
@@ -23,74 +23,19 @@ 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)
|
||||
|
||||
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
|
||||
)
|
||||
add_executable(${PROJECT_NAME} src/main.cpp)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
microxrcedds_agent
|
||||
fastcdr
|
||||
fastrtps
|
||||
$<$<BOOL:$<PLATFORM_ID:Linux>>:rt>
|
||||
$<$<BOOL:$<PLATFORM_ID:Linux>>:dl>
|
||||
)
|
||||
|
||||
target_compile_options(${PROJECT_NAME}
|
||||
PRIVATE
|
||||
$<$<C_COMPILER_ID:GNU>:-Wall>
|
||||
$<$<C_COMPILER_ID:GNU>:-Wextra>
|
||||
$<$<C_COMPILER_ID:GNU>:-pedantic>
|
||||
microxrcedds_agent
|
||||
$<$<BOOL:$<PLATFORM_ID:Linux>>:rt>
|
||||
$<$<BOOL:$<PLATFORM_ID:Linux>>:dl>
|
||||
)
|
||||
|
||||
set_target_properties(${PROJECT_NAME} PROPERTIES
|
||||
CXX_STANDARD
|
||||
14
|
||||
CXX_STANDARD_REQUIRED
|
||||
YES
|
||||
)
|
||||
|
||||
set_target_properties(${PROJECT_NAME} PROPERTIES
|
||||
CXX_STANDARD
|
||||
14
|
||||
11
|
||||
CXX_STANDARD_REQUIRED
|
||||
YES
|
||||
)
|
||||
@@ -134,11 +79,12 @@ if(UROSAGENT_GENERATE_PROFILE)
|
||||
get_filename_component(_COLCON_CALL_DIR "${_COLCON_CALL_DIR}" DIRECTORY)
|
||||
|
||||
set(_PYTHON_SCRIPT_HEAD
|
||||
"import os
|
||||
import sys
|
||||
sys.path.append('${_OUTPUT_PATH}')
|
||||
from ${_PYTHON_PKG_TOOL} import *
|
||||
"
|
||||
"
|
||||
import os\n
|
||||
import sys\n
|
||||
sys.path.append('${_OUTPUT_PATH}')\n
|
||||
from ${_PYTHON_PKG_TOOL} import *\n
|
||||
"
|
||||
)
|
||||
|
||||
file(
|
||||
@@ -251,4 +197,4 @@ from ${_PYTHON_PKG_TOOL} import *
|
||||
set (_XmlDoc "<profiles>\n${_XmlDoc}</profiles>\n")
|
||||
|
||||
file(WRITE "${_DEFAULT_FASTRTPS_PROFILES_PATH}" "${_XmlDoc}")
|
||||
endif()
|
||||
endif()
|
||||
@@ -1,49 +0,0 @@
|
||||
// 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 <uxr/agent/AgentInstance.hpp>
|
||||
#include <uxr/agent/middleware/Middleware.hpp>
|
||||
#include <uxr/agent/middleware/utils/Callbacks.hpp>
|
||||
|
||||
#include <agent/graph_manager/graph_manager.hpp>
|
||||
// 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::GraphManager> graph_manager_;
|
||||
};
|
||||
|
||||
} // namespace agent
|
||||
} // namespace uros
|
||||
#endif // _UROS_AGENT_AGENT_HPP
|
||||
@@ -1,322 +0,0 @@
|
||||
// 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 <fastrtps/TopicDataType.h>
|
||||
#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 <fastrtps/publisher/PublisherListener.h>
|
||||
#include <fastrtps/rtps/common/MatchingInfo.h>
|
||||
|
||||
#include <fastdds/dds/domain/DomainParticipant.hpp>
|
||||
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
|
||||
#include <fastdds/dds/domain/DomainParticipantListener.hpp>
|
||||
#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
|
||||
#include <fastdds/dds/publisher/DataWriter.hpp>
|
||||
#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
|
||||
#include <fastdds/dds/publisher/Publisher.hpp>
|
||||
#include <fastdds/dds/subscriber/DataReader.hpp>
|
||||
#include <fastdds/dds/subscriber/qos/DataReaderQos.hpp>
|
||||
#include <fastdds/dds/subscriber/SampleInfo.hpp>
|
||||
#include <fastdds/dds/subscriber/Subscriber.hpp>
|
||||
#include <fastdds/dds/subscriber/DataReaderListener.hpp>
|
||||
|
||||
#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 <uxr/agent/types/XRCETypes.hpp>
|
||||
|
||||
#include <agent/graph_manager/graph_typesupport.hpp>
|
||||
#include <agent/utils/demangle.hpp>
|
||||
|
||||
#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 <typename Info>
|
||||
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 <typename FastDDSQos>
|
||||
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<ParticipantListener> participant_listener_;
|
||||
std::unique_ptr<DatareaderListener> datareader_listener_;
|
||||
|
||||
std::unique_ptr<eprosima::fastdds::dds::TypeSupport> participant_info_typesupport_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::TypeSupport> microros_graph_info_typesupport_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::DomainParticipant> participant_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::Publisher> publisher_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::Subscriber> subscriber_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_discovery_topic_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::Topic> ros_to_microros_graph_topic_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_discovery_datawriter_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::DataWriter> ros_to_microros_graph_datawriter_;
|
||||
std::unique_ptr<eprosima::fastdds::dds::DataReader> ros_discovery_datareader_;
|
||||
};
|
||||
|
||||
} // namespace graph_manager
|
||||
} // namespace agent
|
||||
} // namespace uros
|
||||
|
||||
#endif // _UROS_AGENT_GRAPH_MANAGER_HPP
|
||||
@@ -1,127 +0,0 @@
|
||||
// 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 <fastrtps/TopicDataType.h>
|
||||
#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 <fastrtps/publisher/PublisherListener.h>
|
||||
#include <fastrtps/rtps/common/MatchingInfo.h>
|
||||
|
||||
#include <fastdds/dds/domain/DomainParticipant.hpp>
|
||||
#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
|
||||
|
||||
#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<uint32_t()> 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<uint32_t()> 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_
|
||||
@@ -1,125 +0,0 @@
|
||||
// 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 <string>
|
||||
|
||||
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_
|
||||
@@ -164,20 +164,21 @@ def generate_XML(args):
|
||||
# Generate source file path
|
||||
src_file = os.path.join(srcs_dir, "%s_%s_%s.xml" % (spec.base_type.pkg_name, subfolder, spec.msg_name))
|
||||
|
||||
|
||||
# Data writer
|
||||
file_content = " <dds>\n"
|
||||
file_content += " <data_writer profile_name=\"%s_%s_%s_p\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
#file_content = " <dds>\n"
|
||||
file_content = " <data_writer profile_name=\"%s_%s_%s_p\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <topic profile_name=\"%s_%s_%s_t\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <kind>NO_KEY</kind>\n"
|
||||
file_content += " <name>%s%s_%s_%s</name>\n" % (ros2_prefix, spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <dataType>%s::%s::dds_::%s_</dataType>\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " </topic>\n"
|
||||
file_content += " </data_writer>\n"
|
||||
file_content += " </dds>\n"
|
||||
#file_content += " </dds>\n"
|
||||
|
||||
|
||||
# Data reader
|
||||
file_content += " <dds>\n"
|
||||
#file_content += " <dds>\n"
|
||||
file_content += " <data_reader profile_name=\"%s_%s_%s_s\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <topic profile_name=\"%s_%s_%s_t\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <kind>NO_KEY</kind>\n"
|
||||
@@ -185,16 +186,16 @@ def generate_XML(args):
|
||||
file_content += " <dataType>%s::%s::dds_::%s_</dataType>\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " </topic>\n"
|
||||
file_content += " </data_reader>\n"
|
||||
file_content += " </dds>\n"
|
||||
#file_content += " </dds>\n"
|
||||
|
||||
|
||||
# Topic
|
||||
file_content += " <dds>\n"
|
||||
#file_content += " <dds>\n"
|
||||
file_content += " <topic profile_name=\"%s_%s_%s_t\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <name>%s%s_%s_%s</name>\n" % (ros2_prefix, spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <dataType>%s::%s::dds_::%s_</dataType>\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " </topic>\n"
|
||||
file_content += " </dds>\n"
|
||||
#file_content += " </dds>\n"
|
||||
|
||||
|
||||
# Write file content
|
||||
|
||||
@@ -8,22 +8,12 @@
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>rosidl_default_generators</test_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<buildtool_export_depend>ament_cmake</buildtool_export_depend>
|
||||
|
||||
<depend>microxrcedds_agent</depend>
|
||||
<depend>rmw</depend>
|
||||
<depend>rcutils</depend>
|
||||
<depend>rmw_fastrtps_shared_cpp</depend>
|
||||
<depend>rmw_dds_common</depend>
|
||||
<depend>micro_ros_msgs</depend>
|
||||
|
||||
<test_depend>rosidl_typesupport_fastrtps_cpp</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>builtin_interfaces</test_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
|
||||
@@ -1,173 +0,0 @@
|
||||
// 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_(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
bool Agent::create(
|
||||
int argc,
|
||||
char** argv)
|
||||
{
|
||||
bool result = xrce_dds_agent_instance_.create(argc, argv);
|
||||
if (result)
|
||||
{
|
||||
graph_manager_.reset(new 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));
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void Agent::run()
|
||||
{
|
||||
return xrce_dds_agent_instance_.run();
|
||||
}
|
||||
|
||||
} // namespace agent
|
||||
} // namespace uros
|
||||
#endif // _UROS_AGENT_AGENT_CPP
|
||||
@@ -1,688 +0,0 @@
|
||||
// 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
|
||||
@@ -1,211 +0,0 @@
|
||||
// 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_
|
||||
@@ -1,191 +0,0 @@
|
||||
// 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,17 +12,76 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <agent/Agent.hpp>
|
||||
#include <uxr/agent/utils/CLI.hpp>
|
||||
#include <csignal>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
uros::agent::Agent micro_ros_agent;
|
||||
|
||||
if (!micro_ros_agent.create(argc, argv))
|
||||
#ifndef _WIN32
|
||||
sigset_t signals;
|
||||
sigemptyset(&signals);
|
||||
if(sigaddset(&signals, SIGINT) && sigaddset(&signals, SIGTERM))
|
||||
{
|
||||
return 1;
|
||||
std::cerr << "Wrong signalset" << std::endl;
|
||||
std::exit(EXIT_FAILURE);
|
||||
}
|
||||
micro_ros_agent.run();
|
||||
sigprocmask( SIG_BLOCK, &signals, nullptr );
|
||||
#endif
|
||||
|
||||
|
||||
/* CLI application. */
|
||||
CLI::App app("micro-ROS Agent");
|
||||
app.require_subcommand(1, 1);
|
||||
app.get_formatter()->column_width(42);
|
||||
|
||||
/* CLI subcommands. */
|
||||
eprosima::uxr::cli::UDPv4Subcommand udpv4_subcommand(app);
|
||||
eprosima::uxr::cli::UDPv6Subcommand udpv6_subcommand(app);
|
||||
eprosima::uxr::cli::TCPv4Subcommand tcpv4_subcommand(app);
|
||||
eprosima::uxr::cli::TCPv6Subcommand tcpv6_subcommand(app);
|
||||
#if !defined(_WIN32) && !defined(__VXWORKS__)
|
||||
eprosima::uxr::cli::TermiosSubcommand serial_subcommand(app);
|
||||
eprosima::uxr::cli::PseudoTerminalSubcommand pseudo_serial_subcommand(app);
|
||||
#endif
|
||||
eprosima::uxr::cli::ExitSubcommand exit_subcommand(app);
|
||||
|
||||
/* CLI parse. */
|
||||
std::string cli_input{};
|
||||
for (int i = 1; i < argc; ++i)
|
||||
{
|
||||
cli_input.append(argv[i]);
|
||||
cli_input.append(" ");
|
||||
}
|
||||
|
||||
while (true)
|
||||
{
|
||||
try
|
||||
{
|
||||
app.parse(cli_input);
|
||||
break;
|
||||
}
|
||||
catch (const CLI::ParseError& e)
|
||||
{
|
||||
app.exit(e);
|
||||
std::cin.clear();
|
||||
std::cout << std::endl;
|
||||
std::cout << "Enter command: ";
|
||||
std::getline(std::cin, cli_input);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef _WIN32
|
||||
/* Waiting until exit. */
|
||||
std::cin.clear();
|
||||
char exit_flag = 0;
|
||||
while ('q' != exit_flag)
|
||||
{
|
||||
std::cin >> exit_flag;
|
||||
}
|
||||
#else
|
||||
/* Wait for SIGTERM/SIGINT instead, as reading from stdin may be redirected to /dev/null. */
|
||||
int n_signal = 0;
|
||||
sigwait(&signals, &n_signal);
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user