Add message lost status event demo using rclcpp (#453)

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

Author: ivanpauno

quality_of_service_demo/README.md

@@ -147,3 +147,33 @@ Notice that "Deadline missed" is constantly being printed even though the publis
 because Deadline and Liveliness QoS Policies are unrelated.
 **Press** `s` in the publisher's terminal again to start publishing messages again,
 and the "Deadline missed" messages will no longer be printed.
+
+## Message lost status event demo
+
+This demo shows how to get a notification when a subscription loses a message.
+
+The feature is available in some rmw implementations: rmw_cyclonedds, rmw_connext.
+CycloneDDS partially implements the feature and it only triggers the event under some limited circumstances, thus it's recommended to try the demo with Connext.
+
+To run the demo:
+```
+export RMW_IMPLEMENTATION = rmw_connext_cpp
+ros2 run quality_of_service_demo_cpp message_lost_listener &
+# -s allows specifying the message size in KiB. This argument is optional, defaults to 8192KiB.
+ros2 run quality_of_service_demo_cpp message_lost_talker -s 8192
+```
+
+Example output:
+```
+[INFO] [1593021627.113051194] [message_lost_talker]: Publishing an image, sent at [1593021627.113023]
+[INFO] [1593021630.114633564] [message_lost_talker]: Publishing an image, sent at [1593021630.114625]
+[INFO] [1593021633.158548336] [message_lost_talker]: Publishing an image, sent at [1593021633.158538]
+[INFO] [1593021635.971678672] [MessageLostListener]: Some messages were lost:
+>       Number of new lost messages: 1
+>       Total number of messages lost: 1
+[INFO] [1593021636.130686719] [message_lost_talker]: Publishing an image, sent at [1593021636.130677]
+[INFO] [1593021636.601466840] [MessageLostListener]: I heard an image. Message single trip latency: [3.442907]
+[INFO] [1593021639.129067211] [message_lost_talker]: Publishing an image, sent at [1593021639.129058]
+```
+
+For information about how to tune QoS settings for large messages see [DDS tuning](https://index.ros.org/doc/ros2/Troubleshooting/DDS-tuning/).

quality_of_service_demo/rclcpp/CMakeLists.txt

@@ -13,8 +13,10 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(rcutils)
 find_package(rmw REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 
 include_directories(include)
@@ -40,6 +42,31 @@ qos_demo_executable(interactive_publisher)
 qos_demo_executable(interactive_subscriber)
 qos_demo_executable(incompatible_qos)
 
+add_library(message_lost SHARED
+  src/message_lost_listener.cpp
+  src/message_lost_talker.cpp)
+ament_target_dependencies(message_lost
+  "rclcpp"
+  "rclcpp_components"
+  "rcutils"
+  "sensor_msgs"
+)
+target_compile_definitions(message_lost
+  PRIVATE "QUALITY_OF_SERVICE_DEMO_BUILDING_DLL")
+rclcpp_components_register_node(message_lost
+  PLUGIN "quality_of_service_demo::MessageLostListener"
+  EXECUTABLE message_lost_listener)
+rclcpp_components_register_node(message_lost
+  PLUGIN "quality_of_service_demo::MessageLostTalker"
+  EXECUTABLE message_lost_talker)
+
+install(TARGETS
+  message_lost
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

quality_of_service_demo/rclcpp/include/quality_of_service_demo/visibility_control.h

@@ -0,0 +1,58 @@
+// 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.
+
+#ifndef QUALITY_OF_SERVICE_DEMO__VISIBILITY_CONTROL_H_
+#define QUALITY_OF_SERVICE_DEMO__VISIBILITY_CONTROL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define QUALITY_OF_SERVICE_DEMO_EXPORT __attribute__ ((dllexport))
+    #define QUALITY_OF_SERVICE_DEMO_IMPORT __attribute__ ((dllimport))
+  #else
+    #define QUALITY_OF_SERVICE_DEMO_EXPORT __declspec(dllexport)
+    #define QUALITY_OF_SERVICE_DEMO_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef QUALITY_OF_SERVICE_DEMO_BUILDING_DLL
+    #define QUALITY_OF_SERVICE_DEMO_PUBLIC QUALITY_OF_SERVICE_DEMO_EXPORT
+  #else
+    #define QUALITY_OF_SERVICE_DEMO_PUBLIC QUALITY_OF_SERVICE_DEMO_IMPORT
+  #endif
+  #define QUALITY_OF_SERVICE_DEMO_PUBLIC_TYPE QUALITY_OF_SERVICE_DEMO_PUBLIC
+  #define QUALITY_OF_SERVICE_DEMO_LOCAL
+#else
+  #define QUALITY_OF_SERVICE_DEMO_EXPORT __attribute__ ((visibility("default")))
+  #define QUALITY_OF_SERVICE_DEMO_IMPORT
+  #if __GNUC__ >= 4
+    #define QUALITY_OF_SERVICE_DEMO_PUBLIC __attribute__ ((visibility("default")))
+    #define QUALITY_OF_SERVICE_DEMO_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define QUALITY_OF_SERVICE_DEMO_PUBLIC
+    #define QUALITY_OF_SERVICE_DEMO_LOCAL
+  #endif
+  #define QUALITY_OF_SERVICE_DEMO_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // QUALITY_OF_SERVICE_DEMO__VISIBILITY_CONTROL_H_

quality_of_service_demo/rclcpp/package.xml

@@ -15,16 +15,20 @@
 
   <build_depend>example_interfaces</build_depend>
   <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_components</build_depend>
   <build_depend>rcutils</build_depend>
   <build_depend>rmw</build_depend>
   <build_depend>rmw_implementation_cmake</build_depend>
+  <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
 
   <exec_depend>example_interfaces</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_components</exec_depend>
   <exec_depend>rcutils</exec_depend>
   <exec_depend>rmw</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

quality_of_service_demo/rclcpp/src/message_lost_listener.cpp

@@ -0,0 +1,65 @@
+// 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/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 MessageLostListener : public rclcpp::Node
+{
+public:
+  QUALITY_OF_SERVICE_DEMO_PUBLIC
+  explicit MessageLostListener(const rclcpp::NodeOptions & options)
+  : Node("MessageLostListener", options)
+  {
+    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());
+      };
+    // Create a subscription to the topic which can be matched with one or more compatible ROS
+    // publishers.
+    // Note that not all publishers on the same topic with the same type will be compatible:
+    // they must have compatible Quality of Service policies.
+    rclcpp::SubscriptionOptions sub_opts;
+    sub_opts.event_callbacks.message_lost_callback =
+      [logger = this->get_logger()](rclcpp::QOSMessageLostInfo & info)
+      {
+        RCLCPP_INFO_STREAM(
+          logger,
+          "Some messages were lost:\n>\tNumber of new lost messages: " <<
+            info.total_count_change << " \n>\tTotal number of messages lost: " <<
+            info.total_count);
+      };
+    sub_ = create_subscription<sensor_msgs::msg::Image>(
+      "message_lost_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::MessageLostListener)

quality_of_service_demo/rclcpp/src/message_lost_talker.cpp

@@ -0,0 +1,115 @@
+// 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
+{
+void print_usage()
+{
+  std::cout <<
+    "Usage: message_lost_talker [-h] [-s SIZE]\n\n"
+    "optional arguments:\n"
+    "\t-h:                           Print this help message.\n"
+    "\t-s <message_size>:            Message size in KiB, default to 8192 KiB" <<
+    std::endl;
+}
+
+class MessageLostTalker : public rclcpp::Node
+{
+public:
+  QUALITY_OF_SERVICE_DEMO_PUBLIC
+  explicit MessageLostTalker(const rclcpp::NodeOptions & options)
+  : Node("message_lost_talker", options),
+    message_size_(8u * 1024u * 1024u)  // 8MB
+  {
+    const std::vector<std::string> & args = this->get_node_options().arguments();
+    if (args.size()) {
+      // Argument usage
+      if (args.cend() != std::find(args.cbegin(), args.cend(), "-h")) {
+        print_usage();
+        // TODO(ivanpauno): Update the rclcpp_components template to be able to handle
+        // exceptions. Raise one here, so stack unwinding happens gracefully.
+        std::exit(0);
+      }
+      // Argument: message size
+      auto opt_it = std::find(args.cbegin(), args.cend(), "-s");
+      if (opt_it != args.cend()) {
+        ++opt_it;
+        if (opt_it == args.cend()) {
+          print_usage();
+          std::cout << "\n-s must be followed by a possitive integer" << std::endl;
+          // TODO(ivanpauno): Update the rclcpp_components template to be able to handle
+          // exceptions. Raise one here, so stack unwinding happens gracefully.
+          std::exit(0);
+        }
+        std::istringstream input_stream(*opt_it);
+        input_stream >> message_size_;
+        if (!input_stream) {
+          print_usage();
+          std::cout << "\n-s must be followed by a possitive integer, got: '" <<
+            *opt_it << "'" << std::endl;
+          // TODO(ivanpauno): Update the rclcpp_components template to be able to handle
+          // exceptions. Raise one here, so stack unwinding happens gracefully.
+          std::exit(0);
+        }
+        message_size_ *= 1024uL;
+      }
+    }
+    auto publish_message =
+      [this]() -> void
+      {
+        for (size_t i = 0; i < message_size_; ++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_);
+      };
+    pub_ = this->create_publisher<sensor_msgs::msg::Image>("message_lost_chatter", 1);
+
+    // Use a timer to schedule periodic message publishing.
+    timer_ = this->create_wall_timer(3s, publish_message);
+  }
+
+private:
+  size_t message_size_;
+  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::MessageLostTalker)