diff --git a/source/Tutorials.rst b/source/Tutorials.rst index 8d9aee451f1..ad1b45eef2e 100644 --- a/source/Tutorials.rst +++ b/source/Tutorials.rst @@ -69,6 +69,7 @@ Advanced Tutorials/Topics/Topic-Statistics-Tutorial Tutorials/Discovery-Server/Discovery-Server Tutorials/Allocator-Template-Tutorial + Tutorials/FastDDS-Configuration/FastDDS-Configuration Miscellaneous ------------- diff --git a/source/Tutorials/FastDDS-Configuration/FastDDS-Configuration.rst b/source/Tutorials/FastDDS-Configuration/FastDDS-Configuration.rst new file mode 100644 index 00000000000..4d4d4183f9a --- /dev/null +++ b/source/Tutorials/FastDDS-Configuration/FastDDS-Configuration.rst @@ -0,0 +1,678 @@ +.. redirect-from:: + + FastDDS-Configuration + +Unlock all the potential of Fast-DDS as ROS 2 middleware [community-contributed] +================================================================================ + +**Goal:** This tutorial will show how to use the extended configuration capabilities of Fast DDS in ROS 2. + +**Tutorial level:** Intermediate + +**Time:** 20 minutes + +.. contents:: Table of Contents + :depth: 2 + :local: + +Background +---------- + +The interface between the ROS 2 stack and *Fast DDS* is provided by the ROS 2 middleware implementation `rmw_fastrtps `_. +This implementation is available in all ROS 2 distributions, both from binaries and from sources. + +ROS 2 RMW only allows for the configuration of certain middleware QoS +(see `ROS 2 QoS policies `_). +However, ``rmw_fastrtps`` offers extended configuration capabilities +to take full advantage of the features in *Fast DDS*. +This tutorial will guide you through a series of examples explaining how to use XML files to unlock this extended configuration. + +In order to get more information about using *Fast DDS* on ROS 2, please check the `following documentation `__. + + +Prerequisites +------------- + +This tutorial does not work with ROS 2 Foxy. +For this tutorial to work with ROS 2 Foxy the [ros2/rmw_fastrtps#335](https://github.com/ros2/rmw_fastrtps/pull/335) and [ros2/rmw_fastrtps#497](https://github.com/ros2/rmw_fastrtps/pull/497) should be backported to the foxy branch. +Right now, this tutorial only works in ROS 2 Rolling and newer installations. + +This tutorial assumes that you know how to :ref:`create a package `. +It also assumes you know how to write a :ref:`simple publisher and subscriber` and a :ref:`simple service and client`. +Although the examples are implemented on C++, the same concepts apply to Python packages. + + +Mixing synchronous and asynchronous publications in the same node +----------------------------------------------------------------- + +On this first example, a node with two publishers, one of them with synchronous publication mode and the other one with asynchronous publication mode, will be created. + +Create the node with the publishers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Start creating the node with both publishers. +First, create a new package named ``sync_async_node_example_cpp``. +Then, add a file named ``sync_async_writer.cpp`` to thepackage, with the following content: + +.. code-block:: C++ + + #include + #include + #include + #include + + #include "rclcpp/rclcpp.hpp" + #include "std_msgs/msg/string.hpp" + + using namespace std::chrono_literals; + + class SyncAsyncPublisher : public rclcpp::Node + { + public: + SyncAsyncPublisher() + : Node("sync_async_publisher"), count_(0) + { + sync_publisher_ = this->create_publisher("sync_topic", 10); + async_publisher_ = this->create_publisher("async_topic", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&SyncAsyncPublisher::timer_callback, this)); + } + + private: + void timer_callback() + { + auto sync_message = std_msgs::msg::String(); + sync_message.data = "SYNC: Hello, world! " + std::to_string(count_); + RCLCPP_INFO(this->get_logger(), "Synchronously publishing: '%s'", sync_message.data.c_str()); + sync_publisher_->publish(sync_message); + + auto async_message = std_msgs::msg::String(); + async_message.data = "ASYNC: Hello, world! " + std::to_string(count_); + RCLCPP_INFO(this->get_logger(), "Asynchronously publishing: '%s'", async_message.data.c_str()); + async_publisher_->publish(async_message); + + count_++; + } + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr async_publisher_; + rclcpp::Publisher::SharedPtr sync_publisher_; + size_t count_; + }; + + int main(int argc, char * argv[]) + { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; + } + + +Note that the synchronous publisher will be publishing on topic ``sync_topic``, while the asynchronous one will be publishing on topic ``async_topic``. + +Add a new target on the ``CMakeLists.txt`` file of your package with this code and name it ``SyncAsyncWriter``, so that you can run the node using ``ros2 run``. +If this node is built and run now, both publishers will behave the same, publishing asynchronously in both topics, because this is the default publication mode. +The default publication mode configuration can be changed on runtime during the node launching, using an XML file. + +Create the XML file with the profile configuration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Create a file with name ``SyncAsync.xml`` and the following content: + +.. code-block:: XML + + + + + + + DYNAMIC + + + + + DYNAMIC + + + SYNCHRONOUS + + + + + + + DYNAMIC + + + ASYNCHRONOUS + + + + + + +Note that several publisher profiles are defined. +A default profile which is defined setting the ``is_default_profile`` to ``true``, and two profiles with names that coincide with those of the previously defined topics: ``sync_topic`` and another one for ``async_topic``. +These last two profiles set the publication mode to ``SYNCHRONOUS`` or ``ASYNCHRONOUS`` accordingly. +Note also that all profiles specify a ``historyMemoryPolicy`` value, which is needed for the example to work, and the reason will be explained later on this tutorial. + +Execute the publisher node +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +You will need to export the following environment variables for the XML to be loaded: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + export RMW_FASTRTPS_USE_QOS_FROM_XML=1 + export FASTRTPS_DEFAULT_PROFILES_FILE=path/to/SyncAsync.xml + + .. group-tab:: macOS + + .. code-block:: console + + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + export RMW_FASTRTPS_USE_QOS_FROM_XML=1 + export FASTRTPS_DEFAULT_PROFILES_FILE=path/to/SyncAsync.xml + + .. group-tab:: Windows + + .. code-block:: console + + SET RMW_IMPLEMENTATION=rmw_fastrtps_cpp + SET RMW_FASTRTPS_USE_QOS_FROM_XML=1 + SET FASTRTPS_DEFAULT_PROFILES_FILE=path/to/SyncAsync.xml + +Finally, ensure you have sourced your setup files and run the node: + +.. code-block:: console + + ros2 run sync_async_node_example_cpp SyncAsyncWriter + +You should see the publishers sending the data from the publishing node, like so: + +.. code-block:: console + + [INFO] [1612972049.994630332] [sync_async_publisher]: Synchronously publishing: 'SYNC: Hello, world! 0' + [INFO] [1612972049.995097767] [sync_async_publisher]: Asynchronously publishing: 'ASYNC: Hello, world! 0' + [INFO] [1612972050.494478706] [sync_async_publisher]: Synchronously publishing: 'SYNC: Hello, world! 1' + [INFO] [1612972050.494664334] [sync_async_publisher]: Asynchronously publishing: 'ASYNC: Hello, world! 1' + [INFO] [1612972050.994368474] [sync_async_publisher]: Synchronously publishing: 'SYNC: Hello, world! 2' + [INFO] [1612972050.994549851] [sync_async_publisher]: Asynchronously publishing: 'ASYNC: Hello, world! 2' + +Now you have a synchronous publisher and an asynchronous publisher running inside the same node. + + +Create a node with the subscribers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Next, a new node with the subscribers that will listen to the ``sync_topic`` and ``async_topic`` publications is going to be created. +On a new source file named ``sync_async_reader.cpp`` write the following content: + +.. code-block:: C++ + + #include + #include + + #include "rclcpp/rclcpp.hpp" + #include "std_msgs/msg/string.hpp" + + using std::placeholders::_1; + + class SyncAsyncSubscriber : public rclcpp::Node + { + public: + + SyncAsyncSubscriber() + : Node("sync_async_subscriber") + { + sync_subscription_ = this->create_subscription( + "sync_topic", 10, std::bind(&SyncAsyncSubscriber::topic_callback, this, _1)); + async_subscription_ = this->create_subscription( + "async_topic", 10, std::bind(&SyncAsyncSubscriber::topic_callback, this, _1)); + } + + private: + + void topic_callback(const std_msgs::msg::String::SharedPtr msg) const + { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + } + + rclcpp::Subscription::SharedPtr sync_subscription_; + rclcpp::Subscription::SharedPtr async_subscription_; + }; + + int main(int argc, char * argv[]) + { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; + } + + + +Add a new target on the ``CMakeLists.txt`` file linked to this code and name it ``SyncAsyncReader``. + +Add the subscriber profiles to the the XML +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Configuration profiles for the subscribers can be added in the same XML file. +For the moment, only the default profile will be set. +The subscriber profiles will be extended later on. +Open the ``SyncAsync.xml`` file and add the following profiles inside the ```` tag: + +.. code-block:: XML + + + + DYNAMIC + + + +Execute the subscriber node +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +With the publisher node running in one terminal, open another one and export the required environment variables for the XML to be loaded: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + export RMW_FASTRTPS_USE_QOS_FROM_XML=1 + export FASTRTPS_DEFAULT_PROFILES_FILE=path/to/SyncAsync.xml + + .. group-tab:: macOS + + .. code-block:: console + + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + export RMW_FASTRTPS_USE_QOS_FROM_XML=1 + export FASTRTPS_DEFAULT_PROFILES_FILE=path/to/SyncAsync.xml + + .. group-tab:: Windows + + .. code-block:: console + + SET RMW_IMPLEMENTATION=rmw_fastrtps_cpp + SET RMW_FASTRTPS_USE_QOS_FROM_XML=1 + SET FASTRTPS_DEFAULT_PROFILES_FILE=path/to/SyncAsync.xml + +Finally, ensure you have sourced your setup files and run the node: + +.. code-block:: console + + ros2 run sync_async_node_example_cpp SyncAsyncReader + +You should see the subscribers receiving the data from the publishing node, like so: + +.. code-block:: console + + [INFO] [1612972054.495429090] [sync_async_subscriber]: I heard: 'SYNC: Hello, world! 10' + [INFO] [1612972054.995410057] [sync_async_subscriber]: I heard: 'ASYNC: Hello, world! 10' + [INFO] [1612972055.495453494] [sync_async_subscriber]: I heard: 'SYNC: Hello, world! 11' + [INFO] [1612972055.995396561] [sync_async_subscriber]: I heard: 'ASYNC: Hello, world! 11' + [INFO] [1612972056.495534818] [sync_async_subscriber]: I heard: 'SYNC: Hello, world! 12' + [INFO] [1612972056.995473953] [sync_async_subscriber]: I heard: 'ASYNC: Hello, world! 12' + + +Analysis of the example +^^^^^^^^^^^^^^^^^^^^^^^ + +Configuration profiles XML +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The XML file defines several configurations for publishers and subscribers. +You can have a default publisher configuration profile and several topic-specific publisher profiles. +The only requirement is that all publisher profiles have a different name and that there is only a single default profile. +The same goes for subscribers. + +In order to define a configuration for a specific topic, just name the profile after the the ROS 2 topic name (like ``/sync_topic`` and ``/async_topic`` in the example), +and ``rmw_fastrtps`` will apply this profile to all publishers and subscribers for that topic. +The default configuration profile is identified by the attribute ``is_default_profile`` set to ``true``, and acts as a fallback profile when there is no other one with a name matching the topic name. + +The environment variable ``FASTRTPS_DEFAULT_PROFILES_FILE`` is used to inform *Fast DDS* the path to the XML file with the configuration profiles to load. + +RMW_FASTRTPS_USE_QOS_FROM_XML +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Among all the configurable attributes, ``rmw_fastrtps`` treats ``publishMode`` and ``historyMemoryPolicy`` differently. +By default, these values are set to ``ASYNCHRONOUS`` and ``PREALLOCATED_WITH_REALLOC`` within the ``rmw_fastrtps`` implementation, and the values set on the XML file are ignored. +In order to use the values in the XML file, the environment variable ``RMW_FASTRTPS_USE_QOS_FROM_XML`` must be set to ``1``. + +However, this entails another caveat: If ``RMW_FASTRTPS_USE_QOS_FROM_XML`` is set, but the XML file does not define +``publishMode`` or ``historyMemoryPolicy``, these attributes take the *Fast DDS* default value instead of the ``rmw_fastrtps`` default value. +This is important, specially for ``historyMemoryPolicy``, because the *Fast DDS* deafult value is ``PREALLOCATED`` which does not work with ROS2 topic data types. +Therefore, in the example, a valid value for this policy has been explicitly set (``DYNAMIC``). + + +Prioritization of rmw_qos_profile_t +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +ROS 2 QoS contained in `rmw_qos_profile_t `_ are always honored, unless set to ``*_SYSTEM_DEFAULT``. +In that case, XML values (or *Fast DDS* default values in the absence of XML ones) are applied. +This means that if any QoS in ``rmw_qos_profile_t`` is set to something other than ``*_SYSTEM_DEFAULT``, the corresponding value in the XML is ignored. + + +Using other FastDDS capabilities with XML +----------------------------------------- + +Although we have created a node with two publishers with different configuration, it is not easy to check that they are behaving differently. +Now that the basics of XML profiles have been covered, let us use them to configure something which has some visual effect on the nodes. +Specifically, a maximum number of matching subscribers on one of the publishers and a partition definition on the other will be set. +Note that these are only very simple examples among all the configuration attributes that can be tuned on ``rmw_fastrtps`` through XML files. +Please refer to `*Fast DDS* documentation `__ to see the whole list of attributes that can be configured through XML files. + +Limiting the number of matching subscribers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Add a maximum number of matched subscribers to the ``/async_topic`` publisher profile. +It should look like this: + +.. code-block:: XML + + + + DYNAMIC + + + ASYNCHRONOUS + + + + 0 + 1 + 1 + + + +The number of matching subscribers is being limited to one. + +Now open three terminals and do not forget to source the setup files and to set the required environment variables. +On the first terminal run the publisher node, and the subscriber node on the other two. +You should see that only the first subscriber node is receiving the messages from both topics. +The second one could not complete the matching process in the ``/async_topic`` because the publisher prevented it, as it had already reached its maximum of matched publishers. +Consequently, only the messages from the ``/sync_topic`` are going to be received in this third terminal: + +.. code-block:: console + + [INFO] [1613127657.088860890] [sync_async_subscriber]: I heard: 'SYNC: Hello, world! 18' + [INFO] [1613127657.588896594] [sync_async_subscriber]: I heard: 'SYNC: Hello, world! 19' + [INFO] [1613127658.088849401] [sync_async_subscriber]: I heard: 'SYNC: Hello, world! 20' + + +Using partitions within the topic +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The partitions feature can be used to control which publishers and subscribers exchange information within the same topic. +Let us change the ``/sync_topic`` publisher to partition ``part1`` and create a new ``/sync_topic`` subscriber which uses partition ``part2``. +Their profiles should now look like this: + +.. code-block:: XML + + + + DYNAMIC + + + SYNCHRONOUS + + + + part1 + + + + + + + + DYNAMIC + + + + part2 + + + + + +Open two terminals. Do not forget to source the setup files and to set the required environment variables. +On the first terminal run the publisher node, and the subscriber node on the other one. +You should see that only the ``/async_topic`` messages are reaching the subscriber. +The ``/sync_topic`` subscriber is not receiving the data as it is in a different partition from the corresponding publisher. + +.. code-block:: console + + [INFO] [1612972054.995410057] [sync_async_subscriber]: I heard: 'ASYNC: Hello, world! 10' + [INFO] [1612972055.995396561] [sync_async_subscriber]: I heard: 'ASYNC: Hello, world! 11' + [INFO] [1612972056.995473953] [sync_async_subscriber]: I heard: 'ASYNC: Hello, world! 12' + + +Configuring a service and a client +---------------------------------- + +Services and clients have a publisher and a subscriber each, that communicate through two different topics. +For example, for a service named ``ping`` there is: + +* A service subscriber listening to requests on ``/rq/ping``. +* A service publisher sending responses on ``/rr/ping``. +* A client publisher sending requests on ``/rq/ping``. +* A client subscriber listening to responses on ``/rr/ping``. + +Although you can use these topic names to set the configuration profiles on the XML, sometimes you may wish to apply the same profile to all services or clients on a node. +Instead of copying the same profile with all topic names generated for all services, you can just create a publisher and subscriber profile pair named ``service``. +The same can be done for clientes creating a pair named ``client``. + + +Create the nodes with the service and client +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Start creating the node with the service. +Add a new source file named ``ping_service.cpp`` on your package with the following content: + +.. code-block:: C++ + + #include "rclcpp/rclcpp.hpp" + #include "example_interfaces/srv/trigger.hpp" + + #include + + void ping(const std::shared_ptr request, + std::shared_ptr response) + { + (void) request; + response->success = true; + RCLCPP_INFO(rclcpp::get_logger("ping_server"), "Incoming request"); + RCLCPP_INFO(rclcpp::get_logger("ping_server"), "Sending back response"); + } + + int main(int argc, char **argv) + { + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("ping_server"); + + rclcpp::Service::SharedPtr service = + node->create_service("ping", &ping); + + RCLCPP_INFO(rclcpp::get_logger("ping_server"), "Ready to serve."); + + rclcpp::spin(node); + rclcpp::shutdown(); + } + +Create the client on a file named ``ping_client.cpp`` with the following content: + +.. code-block:: C++ + + #include "rclcpp/rclcpp.hpp" + #include "example_interfaces/srv/trigger.hpp" + + #include + #include + #include + + using namespace std::chrono_literals; + + int main(int argc, char **argv) + { + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("ping_client"); + rclcpp::Client::SharedPtr client = + node->create_client("ping"); + + auto request = std::make_shared(); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("ping_client"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("ping_client"), "Service not available, waiting again..."); + } + + RCLCPP_INFO(rclcpp::get_logger("ping_client"), "Sending request"); + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(rclcpp::get_logger("ping_client"), "Response received"); + } else { + RCLCPP_ERROR(rclcpp::get_logger("ping_client"), "Failed to call service ping"); + } + + rclcpp::shutdown(); + return 0; + } + +Add a new target on the ``CMakeLists.txt`` file linked to the server code and name it ``ping_service``. +Do the same with a new target named ``ping_client`` and the client code. +Finally, build the package. + + +Create the XML profiles for the service and client +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Create a file with name ``ping.xml`` with the following content: + +.. code-block:: XML + + + + + + + DYNAMIC + + + + + DYNAMIC + + + + + DYNAMIC + + + SYNCHRONOUS + + + + + + + DYNAMIC + + + ASYNCHRONOUS + + + + + + + +This configuration file sets the publication mode to ``SYNCHRONOUS`` on the service and to ``ASYNCHRONOUS`` on the client. +Note that we are only defining the publisher profiles for both the service and the client, but subscriber profiles could be provided too. + + +Execute the nodes +^^^^^^^^^^^^^^^^^ + +Open two terminals and source the setup files on each one. +Then set the required environment variables for the XML to be loaded: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + export RMW_FASTRTPS_USE_QOS_FROM_XML=1 + export FASTRTPS_DEFAULT_PROFILES_FILE=path/to/ping.xml + + .. group-tab:: macOS + + .. code-block:: console + + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + export RMW_FASTRTPS_USE_QOS_FROM_XML=1 + export FASTRTPS_DEFAULT_PROFILES_FILE=path/to/ping.xml + + .. group-tab:: Windows + + .. code-block:: console + + SET RMW_IMPLEMENTATION=rmw_fastrtps_cpp + SET RMW_FASTRTPS_USE_QOS_FROM_XML=1 + SET FASTRTPS_DEFAULT_PROFILES_FILE=path/to/ping.xml + + +On the first terminal run the service node. + +.. code-block:: console + + ros2 run sync_async_node_example_cpp ping_service + +You should see the service waiting for requests: + +.. code-block:: console + + [INFO] [1612977403.805799037] [ping_server]: Ready to serve + +On the second terminal, run the client node. + + +.. code-block:: console + + ros2 run sync_async_node_example_cpp ping_client + +You should see the client sending the request and receiving the response: + +.. code-block:: console + + [INFO] [1612977404.805799037] [ping_client]: Sending request + [INFO] [1612977404.825473835] [ping_client]: Response received + +At the same time, the output in the server console has been updated: + +.. code-block:: console + + [INFO] [1612977403.805799037] [ping_server]: Ready to serve + [INFO] [1612977404.807314904] [ping_server]: Incoming request + [INFO] [1612977404.836405125] [ping_server]: Sending back response +