From de4ad223c2ab346747c39d3c57131a44b8000b9e Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 10 Jun 2025 11:08:51 +0200 Subject: [PATCH 1/2] Fix issue 20, don't publish sensor data when there is a sensor timeout Signed-off-by: Ferry Schoenmakers --- off_highway_can/include/off_highway_can/receiver.hpp | 5 +++-- off_highway_can/src/receiver.cpp | 8 ++++---- off_highway_general_purpose_radar/src/receiver.cpp | 4 ++-- off_highway_radar/src/receiver.cpp | 4 ++-- off_highway_uss/src/receiver.cpp | 4 ++-- 5 files changed, 13 insertions(+), 12 deletions(-) diff --git a/off_highway_can/include/off_highway_can/receiver.hpp b/off_highway_can/include/off_highway_can/receiver.hpp index 712b2d3..3f65574 100644 --- a/off_highway_can/include/off_highway_can/receiver.hpp +++ b/off_highway_can/include/off_highway_can/receiver.hpp @@ -141,6 +141,8 @@ class Receiver : public rclcpp::Node const bool use_fd_{false}; /// Use J1939 protocol, defaults to false if J1939 protocol handling is not implemented for sensor bool use_j1939_{false}; + /// Store the timeout status + bool is_timeout_{false}; private: /** @@ -153,7 +155,7 @@ class Receiver : public rclcpp::Node * * \param stat Status wrapper of diagnostics. */ - void diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) const; + void diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); /** * \brief Declare and get node parameters. @@ -175,7 +177,6 @@ class Receiver : public rclcpp::Node /// information Messages messages_; - double timeout_; double watchdog_frequency_; }; diff --git a/off_highway_can/src/receiver.cpp b/off_highway_can/src/receiver.cpp index e66f7c6..bca4e46 100644 --- a/off_highway_can/src/receiver.cpp +++ b/off_highway_can/src/receiver.cpp @@ -106,15 +106,15 @@ void Receiver::force_diag_update() diag_updater_->force_update(); } -void Receiver::diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) const +void Receiver::diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { using diagnostic_msgs::msg::DiagnosticStatus; - bool timeout = (now() - last_message_received_).seconds() > timeout_; + is_timeout_ = (now() - last_message_received_).seconds() > timeout_; - stat.add("Timeout", timeout); + stat.add("Timeout", is_timeout_); - if (timeout) { + if (is_timeout_) { stat.summary(DiagnosticStatus::ERROR, "Error"); } else { stat.summary(DiagnosticStatus::OK, "Ok"); diff --git a/off_highway_general_purpose_radar/src/receiver.cpp b/off_highway_general_purpose_radar/src/receiver.cpp index 04c3700..1100e76 100644 --- a/off_highway_general_purpose_radar/src/receiver.cpp +++ b/off_highway_general_purpose_radar/src/receiver.cpp @@ -228,7 +228,7 @@ void Receiver::manage_targets() void Receiver::publish_targets() { - if (pub_targets_->get_subscription_count() == 0) { + if (pub_targets_->get_subscription_count() == 0 || is_timeout_) { return; } @@ -247,7 +247,7 @@ void Receiver::publish_targets() void Receiver::publish_pcl() { - if (pub_targets_pcl_->get_subscription_count() == 0) { + if (pub_targets_pcl_->get_subscription_count() == 0 || is_timeout_) { return; } diff --git a/off_highway_radar/src/receiver.cpp b/off_highway_radar/src/receiver.cpp index be6d3ae..138b583 100644 --- a/off_highway_radar/src/receiver.cpp +++ b/off_highway_radar/src/receiver.cpp @@ -222,7 +222,7 @@ void Receiver::manage_objects() void Receiver::publish_objects() { - if (pub_objects_->get_subscription_count() == 0) { + if (pub_objects_->get_subscription_count() == 0 || is_timeout_) { return; } @@ -241,7 +241,7 @@ void Receiver::publish_objects() void Receiver::publish_pcl() { - if (pub_objects_pcl_->get_subscription_count() == 0) { + if (pub_objects_pcl_->get_subscription_count() == 0 || is_timeout_) { return; } diff --git a/off_highway_uss/src/receiver.cpp b/off_highway_uss/src/receiver.cpp index f792477..88b7b83 100644 --- a/off_highway_uss/src/receiver.cpp +++ b/off_highway_uss/src/receiver.cpp @@ -271,7 +271,7 @@ void Receiver::manage_and_publish_objects() void Receiver::publish_objects() { - if (pub_objects_->get_subscription_count() == 0) { + if (pub_objects_->get_subscription_count() == 0 || is_timeout_) { return; } @@ -290,7 +290,7 @@ void Receiver::publish_objects() void Receiver::publish_pcl() { - if (pub_objects_pcl_->get_subscription_count() == 0) { + if (pub_objects_pcl_->get_subscription_count() == 0 || is_timeout_) { return; } From 7509df249bb55f898c25e77a8f33176b7c174ebd Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 10 Jun 2025 11:29:30 +0200 Subject: [PATCH 2/2] Draw conclusion in the watchdog, not in the diagnostics callback Signed-off-by: Ferry Schoenmakers --- off_highway_can/include/off_highway_can/receiver.hpp | 2 +- off_highway_can/src/receiver.cpp | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/off_highway_can/include/off_highway_can/receiver.hpp b/off_highway_can/include/off_highway_can/receiver.hpp index 3f65574..ad44f5e 100644 --- a/off_highway_can/include/off_highway_can/receiver.hpp +++ b/off_highway_can/include/off_highway_can/receiver.hpp @@ -155,7 +155,7 @@ class Receiver : public rclcpp::Node * * \param stat Status wrapper of diagnostics. */ - void diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + void diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) const; /** * \brief Declare and get node parameters. diff --git a/off_highway_can/src/receiver.cpp b/off_highway_can/src/receiver.cpp index bca4e46..dbf6584 100644 --- a/off_highway_can/src/receiver.cpp +++ b/off_highway_can/src/receiver.cpp @@ -79,7 +79,8 @@ void Receiver::stop() void Receiver::callback_watchdog() { - if ((now() - last_message_received_).seconds() > timeout_) { + is_timeout_ = (now() - last_message_received_).seconds() > timeout_; + if (is_timeout_) { RCLCPP_WARN(get_logger(), "Timeout of watchdog for receiving node %s", get_name()); force_diag_update(); last_message_received_ = now(); @@ -106,12 +107,10 @@ void Receiver::force_diag_update() diag_updater_->force_update(); } -void Receiver::diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +void Receiver::diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) const { using diagnostic_msgs::msg::DiagnosticStatus; - is_timeout_ = (now() - last_message_received_).seconds() > timeout_; - stat.add("Timeout", is_timeout_); if (is_timeout_) {