Add demo of how to use qos overrides (#474)

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Author: ivanpauno

quality_of_service_demo/README.md

@@ -178,3 +178,19 @@ Example output:
 ```
 
 For information about how to tune QoS settings for large messages see [DDS tuning](https://index.ros.org/doc/ros2/Troubleshooting/DDS-tuning/).
+
+## Qos overrides
+
+You can use QoS overrides parameters for making QoS profiles configurable when starting a node.
+Create a parameters yaml file, similar to the examples in the `params_file` folder, and run:
+
+```
+# you can use `$(ros2 pkg prefix quality_of_service_demo_cpp)/share/quality_of_service_demo_cpp/params_file/example_qos_overrides.yaml` instead of `/path/to/yaml/file` to use the example installed yaml file.
+ros2 run quality_of_service_demo_cpp qos_overrides_talker --ros-args --params-file /path/to/yaml/file
+```
+
+in another terminal:
+
+```
+ros2 run quality_of_service_demo_cpp qos_overrides_listener --ros-args --params-file /path/to/yaml/file
+```

quality_of_service_demo/rclcpp/CMakeLists.txt

@@ -60,13 +60,34 @@ rclcpp_components_register_node(message_lost
   PLUGIN "quality_of_service_demo::MessageLostTalker"
   EXECUTABLE message_lost_talker)
 
+add_library(qos_overrides SHARED
+  src/qos_overrides_listener.cpp
+  src/qos_overrides_talker.cpp
+)
+ament_target_dependencies(qos_overrides
+  "rclcpp"
+  "rclcpp_components"
+  "rcutils"
+  "sensor_msgs"
+)
+target_compile_definitions(qos_overrides
+  PRIVATE "QUALITY_OF_SERVICE_DEMO_BUILDING_DLL")
+rclcpp_components_register_node(qos_overrides
+  PLUGIN "quality_of_service_demo::QosOverridesListener"
+  EXECUTABLE qos_overrides_listener)
+rclcpp_components_register_node(qos_overrides
+  PLUGIN "quality_of_service_demo::QosOverridesTalker"
+  EXECUTABLE qos_overrides_talker)
+
 install(TARGETS
-  message_lost
+  message_lost qos_overrides
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
+install(DIRECTORY params_file DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME})
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

quality_of_service_demo/rclcpp/params_file/example_qos_overrides.yaml

@@ -0,0 +1,17 @@
+qos_overrides_talker:  # node name
+  ros__parameters:  # All parameters are nested in this key
+    qos_overrides:  # Parameter group for all qos overrides
+      /qos_overrides_chatter:  # Parameter group for the topic
+        publisher:  # Profile for publishers in the topic
+        # publisher_my_id:  # Profile for publishers in the topic with id="my_id"
+          reliability: reliable
+          depth: 9
+          # depth: 11  # Uncomment this line to make the validation callback fail
+qos_overrides_listener:
+  ros__parameters:
+    qos_overrides:
+      /qos_overrides_chatter:
+        subscription:  # Profile for subscriptions in the topic
+        # subscription_my_id:  # Profile for subscriptions in the topic with id="my_id"
+          reliability: reliable
+          depth: 9

quality_of_service_demo/rclcpp/params_file/example_qos_overrides_with_wildcard.yaml

@@ -0,0 +1,10 @@
+/**:
+  ros__parameters:
+    qos_overrides:
+      /qos_overrides_chatter:
+        publisher:
+          reliability: reliable
+          depth: 9
+        subscription:
+          reliability: reliable
+          depth: 9

quality_of_service_demo/rclcpp/src/qos_overrides_listener.cpp

@@ -0,0 +1,91 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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 "rclcpp/qos_overriding_options.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+#include "sensor_msgs/msg/image.hpp"
+
+#include "quality_of_service_demo/visibility_control.h"
+
+namespace quality_of_service_demo
+{
+class QosOverridesListener : public rclcpp::Node
+{
+public:
+  QUALITY_OF_SERVICE_DEMO_PUBLIC
+  explicit QosOverridesListener(const rclcpp::NodeOptions & options)
+  : Node("qos_overrides_listener", options)
+  {
+    // Callback that will be used when a message is received.
+    auto callback =
+      [this](const sensor_msgs::msg::Image::SharedPtr msg) -> void
+      {
+        rclcpp::Time now = this->get_clock()->now();
+        auto diff = now - msg->header.stamp;
+        RCLCPP_INFO(
+          this->get_logger(),
+          "I heard an image. Message single trip latency: [%f]",
+          diff.seconds());
+      };
+    rclcpp::SubscriptionOptions sub_opts;
+    // Update the subscription options to allow reconfigurable qos settings.
+    sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {
+      {
+        // Here all policies that are desired to be reconfigurable are listed.
+        rclcpp::QosPolicyKind::Depth,
+        rclcpp::QosPolicyKind::Durability,
+        rclcpp::QosPolicyKind::History,
+        rclcpp::QosPolicyKind::Reliability,
+      },
+      [](const rclcpp::QoS & qos) {
+        /** This is a qos validation callback, that can optionally be provided.
+         * Here, arbitrary constraints in the final qos profile can be checked.
+         * The function will return true if the user provided qos profile is accepted.
+         * If the profile is not accepted, the user will get an InvalidQosOverridesException.
+         */
+        rclcpp::QosCallbackResult result;
+        result.successful = false;
+        if (qos.depth() > 10u) {
+          result.reason = "expected history depth less or equal than 10";
+          return result;
+        }
+        result.successful = true;
+        return result;
+      }
+      /**
+       * The "id" option is useful when you want to have subscriptions
+       * listening to the same topic with different QoS profiles within a node.
+       * You can try uncommenting and modifying
+       * `qos_overrides./qos_overrides_topic.subscription`
+       * to
+       * `qos_overrides./qos_overrides_topic.subscription_custom_identifier`
+       *
+       * Uncomment the next line to try it.
+       */
+      // , "custom_identifier"
+    };
+    // create the subscription
+    sub_ = create_subscription<sensor_msgs::msg::Image>(
+      "qos_overrides_chatter", 1, callback, sub_opts);
+  }
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
+};
+
+}  // namespace quality_of_service_demo
+
+RCLCPP_COMPONENTS_REGISTER_NODE(quality_of_service_demo::QosOverridesListener)

quality_of_service_demo/rclcpp/src/qos_overrides_talker.cpp

@@ -0,0 +1,112 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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 <chrono>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rcutils/cmdline_parser.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+#include "sensor_msgs/msg/image.hpp"
+
+#include "quality_of_service_demo/visibility_control.h"
+
+using namespace std::chrono_literals;
+
+namespace quality_of_service_demo
+{
+
+class QosOverridesTalker : public rclcpp::Node
+{
+public:
+  QUALITY_OF_SERVICE_DEMO_PUBLIC
+  explicit QosOverridesTalker(const rclcpp::NodeOptions & options)
+  : Node("qos_overrides_talker", options)
+  {
+    // Timer callback
+    auto publish_message =
+      [this]() -> void
+      {
+        for (size_t i = 0; i < 1u * 1024u * 1024u; ++i) {
+          msg_.data.push_back(0);
+        }
+        rclcpp::Time now = this->get_clock()->now();
+        msg_.header.stamp = now;
+        RCLCPP_INFO(
+          this->get_logger(),
+          "Publishing an image, sent at [%f]",
+          now.seconds());
+
+        pub_->publish(msg_);
+      };
+
+    rclcpp::PublisherOptions pub_opts;
+    // Update the subscription options to allow reconfigurable qos settings.
+    pub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {
+      {
+        // Here all policies that are desired to be reconfigurable are listed.
+        rclcpp::QosPolicyKind::Depth,
+        rclcpp::QosPolicyKind::Durability,
+        rclcpp::QosPolicyKind::History,
+        rclcpp::QosPolicyKind::Reliability
+      },
+      [](const rclcpp::QoS & qos) {
+        /** This is a qos validation callback, that can optionally be provided.
+         * Here, arbitrary constraints in the final qos profile can be checked.
+         * The function will return true if the user provided qos profile is accepted.
+         * If the profile is not accepted, the user will get an InvalidQosOverridesException.
+         */
+        rclcpp::QosCallbackResult result;
+        result.successful = false;
+        if (qos.depth() > 10u) {
+          result.reason = "expected history depth less or equal than 10";
+          return result;
+        }
+        result.successful = true;
+        return result;
+      }
+      /**
+       * The "id" option is useful when you want to have subscriptions
+       * listening to the same topic with different QoS profiles within a node.
+       * You can try uncommenting and modifying
+       * `qos_overrides./qos_overrides_topic.subscription`
+       * to
+       * `qos_overrides./qos_overrides_topic.subscription_custom_identifier`
+       *
+       * Uncomment the next line to try it.
+       */
+      // , "custom_identifier"
+    };
+
+    // Create a publisher
+    pub_ = this->create_publisher<sensor_msgs::msg::Image>("qos_overrides_chatter", 1, pub_opts);
+
+    // Use a timer to schedule periodic message publishing.
+    timer_ = this->create_wall_timer(3s, publish_message);
+  }
+
+private:
+  sensor_msgs::msg::Image msg_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}  // namespace quality_of_service_demo
+
+RCLCPP_COMPONENTS_REGISTER_NODE(quality_of_service_demo::QosOverridesTalker)