diff --git a/quality_of_service_demo/README.md b/quality_of_service_demo/README.md index 869a4c147..cbfeeab7d 100644 --- a/quality_of_service_demo/README.md +++ b/quality_of_service_demo/README.md @@ -156,8 +156,8 @@ and the "Deadline missed" messages will no longer be printed. This demo shows how to get a notification when a subscription loses a message. This feature is not available in all RMW implementations. -`rmw_cyclonedds_cpp` and `rmw_connextdds` do support this feature. -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. +`rmw_zenoh_cpp`, `rmw_cyclonedds_cpp` and `rmw_connextdds` do support this feature. +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 or `rmw_zenoh_cpp`. In one terminal, run a listener ``` diff --git a/quality_of_service_demo/rclcpp/src/message_lost_listener.cpp b/quality_of_service_demo/rclcpp/src/message_lost_listener.cpp index 1ffa95e0e..42ccbc1ec 100644 --- a/quality_of_service_demo/rclcpp/src/message_lost_listener.cpp +++ b/quality_of_service_demo/rclcpp/src/message_lost_listener.cpp @@ -54,8 +54,10 @@ class MessageLostListener : public rclcpp::Node }; // Create the subscription. This will also create an event handler based on the above // subscription options. + rclcpp::QoS qos{1}; + qos.best_effort(); sub_ = create_subscription( - "message_lost_chatter", 1, callback, sub_opts); + "message_lost_chatter", std::move(qos), callback, sub_opts); } private: diff --git a/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp b/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp index c63d8b4e7..b68898929 100644 --- a/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp +++ b/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp @@ -37,6 +37,7 @@ void print_usage() "Usage: message_lost_talker [-h] [-s SIZE]\n\n" "optional arguments:\n" "\t-h: Print this help message.\n" + "\t-r: Timer rate in Hz, default to 0.3 Hz\n" "\t-s : Message size in KiB, default to 8192 KiB" << std::endl; } @@ -47,7 +48,8 @@ class MessageLostTalker : public rclcpp::Node QUALITY_OF_SERVICE_DEMO_PUBLIC explicit MessageLostTalker(const rclcpp::NodeOptions & options) : Node("message_lost_talker", options), - message_size_(8u * 1024u * 1024u) // 8MB + message_size_(8u * 1024u * 1024u), // 8MB + timer_period_(3000) // 3s period { const std::vector & args = this->get_node_options().arguments(); if (args.size()) { @@ -58,13 +60,38 @@ class MessageLostTalker : public rclcpp::Node // exceptions. Raise one here, so stack unwinding happens gracefully. std::exit(0); } + // Argument: timer period + auto opt_it = std::find(args.cbegin(), args.cend(), "-r"); + if (opt_it != args.cend()) { + ++opt_it; + if (opt_it == args.cend()) { + print_usage(); + std::cout << "\n-r must be followed by a positive number" << 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); + double timer_rate; + input_stream >> timer_rate; + if (!input_stream) { + print_usage(); + std::cout << "\n-s must be followed by a positive number, 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); + } + timer_period_ = std::chrono::duration_cast( + std::chrono::duration>(1.0 / timer_rate)); + } // Argument: message size - auto opt_it = std::find(args.cbegin(), args.cend(), "-s"); + 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; + std::cout << "\n-s must be followed by a positive 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); @@ -73,7 +100,7 @@ class MessageLostTalker : public rclcpp::Node input_stream >> message_size_; if (!input_stream) { print_usage(); - std::cout << "\n-s must be followed by a possitive integer, got: '" << + std::cout << "\n-s must be followed by a positive 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. @@ -97,16 +124,19 @@ class MessageLostTalker : public rclcpp::Node pub_->publish(msg_); }; // Create a publisher - pub_ = this->create_publisher("message_lost_chatter", 1); + rclcpp::QoS qos{1}; + qos.reliable(); + pub_ = this->create_publisher("message_lost_chatter", std::move(qos)); // Use a timer to schedule periodic message publishing. - timer_ = this->create_wall_timer(3s, publish_message); + timer_ = this->create_wall_timer(timer_period_, publish_message); } private: size_t message_size_; sensor_msgs::msg::Image msg_; rclcpp::Publisher::SharedPtr pub_; + std::chrono::milliseconds timer_period_; rclcpp::TimerBase::SharedPtr timer_; }; diff --git a/quality_of_service_demo/rclpy/quality_of_service_demo_py/message_lost_listener.py b/quality_of_service_demo/rclpy/quality_of_service_demo_py/message_lost_listener.py index 7a34a739c..33329e6cb 100644 --- a/quality_of_service_demo/rclpy/quality_of_service_demo_py/message_lost_listener.py +++ b/quality_of_service_demo/rclpy/quality_of_service_demo_py/message_lost_listener.py @@ -20,6 +20,7 @@ from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from rclpy.time import Time +from rclpy.qos import QoSReliabilityPolicy, QoSProfile from sensor_msgs.msg import Image @@ -37,11 +38,12 @@ def __init__(self): event_callbacks = SubscriptionEventCallbacks( message_lost=self._message_lost_event_callback) # Create a subscription, passing the previously created event handlers. + qos = QoSProfile(depth=1, reliability=QoSReliabilityPolicy.BEST_EFFORT) self.subscription = self.create_subscription( Image, 'message_lost_chatter', self._message_callback, - 1, + qos, event_callbacks=event_callbacks) def _message_callback(self, message): @@ -49,7 +51,7 @@ def _message_callback(self, message): now = self.get_clock().now() diff = now - Time.from_msg(message.header.stamp) self.get_logger().info( - f'I heard an Image. Message single trip latency: [{diff.nanoseconds}]\n---') + f'I heard an Image. Message single trip latency: [{diff.nanoseconds / 1e9}]\n---') def _message_lost_event_callback(self, message_lost_status): """Log the number of lost messages when the event is triggered."""