From 7e02abda9b3be3d0c03894844a8b19e00ffffb48 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 5 May 2024 16:01:27 -0500 Subject: [PATCH 1/7] Allow timer rate to be configurable Signed-off-by: Yadunund --- .../rclcpp/src/message_lost_listener.cpp | 4 +- .../rclcpp/src/message_lost_talker.cpp | 43 ++++++++++++++++--- .../message_lost_listener.py | 6 ++- 3 files changed, 44 insertions(+), 9 deletions(-) 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..05793c6be 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", 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..5e382764e 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-4: 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,20 @@ 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", 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.""" From 2e537a42b7af795058ce650f33487a29cd8d7f51 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 5 May 2024 16:13:38 -0500 Subject: [PATCH 2/7] Update README Signed-off-by: Yadunund --- quality_of_service_demo/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 ``` From d8e25f31dc5d76aef573aef016d3c95dccd60ee5 Mon Sep 17 00:00:00 2001 From: Yadu Date: Sun, 5 May 2024 18:16:13 -0500 Subject: [PATCH 3/7] Update quality_of_service_demo/rclcpp/src/message_lost_talker.cpp Co-authored-by: Michael Carroll Signed-off-by: Yadu --- quality_of_service_demo/rclcpp/src/message_lost_talker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 5e382764e..ad7217123 100644 --- a/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp +++ b/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp @@ -37,7 +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-4: Timer rate in Hz, default to 0.3 Hz\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; } From 3e25875c6cb98ee73e90fe72af65e437d48f056a Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 5 May 2024 22:02:25 -0500 Subject: [PATCH 4/7] Move qos Signed-off-by: Yadunund --- quality_of_service_demo/rclcpp/src/message_lost_listener.cpp | 2 +- quality_of_service_demo/rclcpp/src/message_lost_talker.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) 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 05793c6be..42ccbc1ec 100644 --- a/quality_of_service_demo/rclcpp/src/message_lost_listener.cpp +++ b/quality_of_service_demo/rclcpp/src/message_lost_listener.cpp @@ -57,7 +57,7 @@ class MessageLostListener : public rclcpp::Node rclcpp::QoS qos{1}; qos.best_effort(); sub_ = create_subscription( - "message_lost_chatter", qos, 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 ad7217123..b68898929 100644 --- a/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp +++ b/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp @@ -126,8 +126,7 @@ class MessageLostTalker : public rclcpp::Node // Create a publisher rclcpp::QoS qos{1}; qos.reliable(); - pub_ = this->create_publisher("message_lost_chatter", qos); - + pub_ = this->create_publisher("message_lost_chatter", std::move(qos)); // Use a timer to schedule periodic message publishing. timer_ = this->create_wall_timer(timer_period_, publish_message); From 22d925f5361888bb873c1d519262c1053a4b5288 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 6 Oct 2025 11:13:07 +0200 Subject: [PATCH 5/7] Apply suggestion from @fujitatomoya MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Tomoya Fujita Signed-off-by: Alejandro Hernández Cordero --- quality_of_service_demo/rclcpp/src/message_lost_talker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b68898929..e8d6a70b4 100644 --- a/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp +++ b/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp @@ -76,7 +76,7 @@ class MessageLostTalker : public rclcpp::Node input_stream >> timer_rate; if (!input_stream) { print_usage(); - std::cout << "\n-s must be followed by a positive number, got: '" << + std::cout << "\n-r 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. From b4099eb42581dbcc6439e187d10069c7ef165c81 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 6 Oct 2025 11:13:46 +0200 Subject: [PATCH 6/7] Apply suggestion from @fujitatomoya MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Tomoya Fujita Signed-off-by: Alejandro Hernández Cordero --- quality_of_service_demo/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quality_of_service_demo/README.md b/quality_of_service_demo/README.md index cbfeeab7d..3a16a8e09 100644 --- a/quality_of_service_demo/README.md +++ b/quality_of_service_demo/README.md @@ -156,7 +156,7 @@ 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_zenoh_cpp`, `rmw_cyclonedds_cpp` and `rmw_connextdds` do support this feature. +`rmw_fastrtps_cpp`, `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 From fa03461a00edbdd573942a1cd68d314e5c2c200e Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Mon, 6 Oct 2025 11:20:30 +0200 Subject: [PATCH 7/7] Review feedback Signed-off-by: Alejandro Hernandez Cordero --- quality_of_service_demo/rclcpp/src/message_lost_talker.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 e8d6a70b4..d8107281f 100644 --- a/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp +++ b/quality_of_service_demo/rclcpp/src/message_lost_talker.cpp @@ -82,6 +82,10 @@ class MessageLostTalker : public rclcpp::Node // exceptions. Raise one here, so stack unwinding happens gracefully. std::exit(0); } + if (timer_rate <= 0) { + std::cout << "\ntimer_rate must be higher than zero" << std::endl; + std::exit(0); + } timer_period_ = std::chrono::duration_cast( std::chrono::duration>(1.0 / timer_rate)); }