diff --git a/off_highway_can/include/off_highway_can/receiver.hpp b/off_highway_can/include/off_highway_can/receiver.hpp index 712b2d3..ad44f5e 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: /** @@ -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..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(); @@ -110,11 +111,9 @@ void Receiver::diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) c { using diagnostic_msgs::msg::DiagnosticStatus; - bool timeout = (now() - last_message_received_).seconds() > timeout_; + stat.add("Timeout", is_timeout_); - stat.add("Timeout", 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; }