Add Node time fallback for headerless messages (take 2)

README.md

@@ -15,7 +15,6 @@ This diagram shows an overview of the architecture:
 
 ![architecture](docs/images/greenwave_diagram.png)
 
-
 For implementation details and inline integration guidance, see
 [`docs/DESIGN_AND_IMPLEMENTATION.md`](docs/DESIGN_AND_IMPLEMENTATION.md).
 

greenwave_monitor/config/example.yaml

@@ -19,6 +19,13 @@
 
 greenwave_monitor:
   ros__parameters:
+    # Controls which timestamp source is used for low-FPS error checks.
+    # Valid values:
+    # - header_with_nodetime_fallback (default): header timestamps when available, otherwise node
+    #   time fallback for headerless topics.
+    # - header_only: use only header/message timestamps (no fallback for headerless topics).
+    # - nodetime_only: always use node/pub-time checks.
+    gw_timestamp_monitor_mode: header_with_nodetime_fallback
     # gw_monitored_topics parameter specifies topics to monitor that do not require expected
     # frequencies or tolerances.
     gw_monitored_topics: ['/string_topic']

greenwave_monitor/include/greenwave_diagnostics.hpp

@@ -35,6 +35,13 @@
 
 namespace greenwave_diagnostics
 {
+enum class TimestampMonitorMode
+{
+  kHeaderWithNodetimeFallback,
+  kHeaderOnly,
+  kNodetimeOnly
+};
+
 namespace constants
 {
 inline constexpr uint64_t kSecondsToNanoseconds = 1000000000ULL;
@@ -51,18 +58,10 @@ inline constexpr int64_t kNonsenseLatencyMs = 365LL * 24LL * 60LL * 60LL * 1000L
 // Configurations for a greenwave diagnostics
 struct GreenwaveDiagnosticsConfig
 {
-  // diagnostics toggle
-  bool enable_diagnostics{false};
-
-  // corresponds to launch arguments
-  bool enable_all_diagnostics{false};
   bool enable_node_time_diagnostics{false};
   bool enable_msg_time_diagnostics{false};
   bool enable_increasing_msg_time_diagnostics{false};
 
-  // enable basic diagnostics for all topics, triggered by an environment variable
-  bool enable_all_topic_diagnostics{false};
-
   // Window size of the mean filter in terms of number of messages received
   int filter_window_size{300};
 
@@ -97,6 +96,7 @@ class GreenwaveDiagnostics
 
     prev_drop_ts_ = rclcpp::Time(0, 0, clock_->get_clock_type());
     prev_noninc_msg_ts_ = rclcpp::Time(0, 0, clock_->get_clock_type());
+    prev_drop_node_ts_ = rclcpp::Time(0, 0, clock_->get_clock_type());
     prev_timestamp_node_us_ = std::numeric_limits<uint64_t>::min();
     prev_timestamp_msg_us_ = std::numeric_limits<uint64_t>::min();
     num_non_increasing_msg_ = 0;
@@ -278,11 +278,14 @@ class GreenwaveDiagnostics
     return message_latency_msg_ms_;
   }
 
-  void setExpectedDt(double expected_hz, double tolerance_percent)
+  void setExpectedDt(
+    double expected_hz, double tolerance_percent,
+    bool enable_node_checks = true, bool enable_msg_checks = true)
   {
     const std::lock_guard<std::mutex> lock(greenwave_diagnostics_mutex_);
-    diagnostics_config_.enable_node_time_diagnostics = true;
-    diagnostics_config_.enable_msg_time_diagnostics = true;
+    diagnostics_config_.enable_node_time_diagnostics = enable_node_checks;
+    diagnostics_config_.enable_msg_time_diagnostics = enable_msg_checks;
+    diagnostics_config_.enable_increasing_msg_time_diagnostics = enable_msg_checks;
 
     // This prevents accidental 0 division in the calculations in case of
     // a direct function call (not from service in greenwave_monitor.cpp)
@@ -314,6 +317,7 @@ class GreenwaveDiagnostics
     const std::lock_guard<std::mutex> lock(greenwave_diagnostics_mutex_);
     diagnostics_config_.enable_node_time_diagnostics = false;
     diagnostics_config_.enable_msg_time_diagnostics = false;
+    diagnostics_config_.enable_increasing_msg_time_diagnostics = false;
 
     diagnostics_config_.expected_dt_us = 0;
     diagnostics_config_.jitter_tolerance_us = 0;
@@ -388,7 +392,7 @@ class GreenwaveDiagnostics
   std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec_;
   rclcpp::Clock::SharedPtr clock_;
   rclcpp::Time t_start_;
-  rclcpp::Time prev_drop_ts_, prev_noninc_msg_ts_;
+  rclcpp::Time prev_drop_ts_, prev_noninc_msg_ts_, prev_drop_node_ts_;
   uint64_t prev_timestamp_node_us_, prev_timestamp_msg_us_;
 
   RollingWindow node_window_;
@@ -423,6 +427,7 @@ class GreenwaveDiagnostics
     const bool missed_deadline_node =
       node_window_.addJitter(abs_jitter_node, diagnostics_config_.jitter_tolerance_us);
     if (missed_deadline_node) {
+      prev_drop_node_ts_ = clock_->now();
       RCLCPP_DEBUG(
         node_.get_logger(),
         "[GreenwaveDiagnostics Node Time]"
@@ -434,6 +439,15 @@ class GreenwaveDiagnostics
         static_cast<int64_t>(diagnostics_config_.jitter_tolerance_us),
         abs_jitter_node, topic_name_.c_str());
     }
+    if (prev_drop_node_ts_.nanoseconds() != 0) {
+      auto time_since_drop = (clock_->now() - prev_drop_node_ts_).seconds();
+      if (time_since_drop < greenwave_diagnostics::constants::kDropWarnTimeoutSeconds) {
+        error_found = true;
+        status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+        update_status_message(status_vec_[0], "FRAME DROP DETECTED (NODE TIME)");
+      }
+    }
+
     return error_found;
   }
 

greenwave_monitor/include/greenwave_monitor.hpp

@@ -23,6 +23,7 @@
 #include <optional>
 #include <string>
 #include <thread>
+#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/rclcpp.hpp"
@@ -88,6 +89,10 @@ class GreenwaveMonitor : public rclcpp::Node
 
   void add_topics_from_parameters();
 
+  greenwave_diagnostics::TimestampMonitorMode timestamp_monitor_mode_{
+    greenwave_diagnostics::TimestampMonitorMode::kHeaderWithNodetimeFallback};
+  std::unordered_map<std::string, bool> topic_has_header_;
+
   std::map<std::string,
     std::unique_ptr<greenwave_diagnostics::GreenwaveDiagnostics>> greenwave_diagnostics_;
   std::vector<std::shared_ptr<rclcpp::GenericSubscription>> subscriptions_;

greenwave_monitor/src/greenwave_monitor.cpp

@@ -21,6 +21,7 @@
 #include <cstring>
 #include <mutex>
 #include <set>
+#include <utility>
 #include <unordered_map>
 
 #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
@@ -34,9 +35,59 @@ namespace constants
 inline constexpr const char * kTopicParamPrefix = "gw_frequency_monitored_topics.";
 inline constexpr const char * kExpectedFrequencySuffix = ".expected_frequency";
 inline constexpr const char * kToleranceSuffix = ".tolerance";
+inline constexpr const char * kTimestampMonitorModeParam = "gw_timestamp_monitor_mode";
+inline constexpr const char * kTimestampModeHeaderWithFallback = "header_with_nodetime_fallback";
+inline constexpr const char * kTimestampModeHeaderOnly = "header_only";
+inline constexpr const char * kTimestampModeNodetimeOnly = "nodetime_only";
 }  // namespace constants
 }  // namespace greenwave_monitor
 
+namespace
+{
+greenwave_diagnostics::TimestampMonitorMode parse_timestamp_monitor_mode(
+  const std::string & mode, rclcpp::Logger logger)
+{
+  using greenwave_diagnostics::TimestampMonitorMode;
+  using greenwave_monitor::constants::kTimestampModeHeaderOnly;
+  using greenwave_monitor::constants::kTimestampModeHeaderWithFallback;
+  using greenwave_monitor::constants::kTimestampModeNodetimeOnly;
+
+  if (mode == kTimestampModeHeaderWithFallback) {
+    return TimestampMonitorMode::kHeaderWithNodetimeFallback;
+  }
+  if (mode == kTimestampModeHeaderOnly) {
+    return TimestampMonitorMode::kHeaderOnly;
+  }
+  if (mode == kTimestampModeNodetimeOnly) {
+    return TimestampMonitorMode::kNodetimeOnly;
+  }
+
+  RCLCPP_WARN(
+    logger,
+    "Invalid value '%s' for gw_timestamp_monitor_mode. Falling back to default."
+    " Allowed values are '%s', '%s', '%s'.",
+    mode.c_str(),
+    kTimestampModeHeaderWithFallback,
+    kTimestampModeHeaderOnly,
+    kTimestampModeNodetimeOnly);
+  return TimestampMonitorMode::kHeaderWithNodetimeFallback;
+}
+
+std::pair<bool, bool> resolve_timestamp_checks(
+  greenwave_diagnostics::TimestampMonitorMode mode, bool topic_has_header)
+{
+  switch (mode) {
+    case greenwave_diagnostics::TimestampMonitorMode::kHeaderWithNodetimeFallback:
+      return topic_has_header ? std::make_pair(false, true) : std::make_pair(true, false);
+    case greenwave_diagnostics::TimestampMonitorMode::kHeaderOnly:
+      return topic_has_header ? std::make_pair(false, true) : std::make_pair(false, false);
+    case greenwave_diagnostics::TimestampMonitorMode::kNodetimeOnly:
+      return std::make_pair(true, false);
+  }
+  return std::make_pair(false, false);
+}
+}  // namespace
+
 GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options)
 : Node("greenwave_monitor",
     rclcpp::NodeOptions(options)
@@ -48,6 +99,15 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options)
   if (!this->has_parameter("gw_monitored_topics")) {
     this->declare_parameter<std::vector<std::string>>("gw_monitored_topics", {""});
   }
+  if (!this->has_parameter(greenwave_monitor::constants::kTimestampMonitorModeParam)) {
+    this->declare_parameter<std::string>(
+      greenwave_monitor::constants::kTimestampMonitorModeParam,
+      greenwave_monitor::constants::kTimestampModeHeaderWithFallback);
+  }
+
+  const auto monitor_mode = this->get_parameter(
+    greenwave_monitor::constants::kTimestampMonitorModeParam).as_string();
+  timestamp_monitor_mode_ = parse_timestamp_monitor_mode(monitor_mode, this->get_logger());
 
   timer_ = this->create_wall_timer(
     1s, std::bind(&GreenwaveMonitor::timer_callback, this));
@@ -176,7 +236,19 @@ void GreenwaveMonitor::handle_set_expected_frequency(
     return;
   }
 
-  msg_diagnostics_obj.setExpectedDt(request->expected_hz, request->tolerance_percent);
+  bool topic_has_header = false;
+  if (auto header_it = topic_has_header_.find(request->topic_name);
+    header_it != topic_has_header_.end())
+  {
+    topic_has_header = header_it->second;
+  }
+  const auto [enable_node, enable_msg] = resolve_timestamp_checks(
+    timestamp_monitor_mode_, topic_has_header);
+  msg_diagnostics_obj.setExpectedDt(
+    request->expected_hz,
+    request->tolerance_percent,
+    enable_node,
+    enable_msg);
 
   response->success = true;
   response->message = "Successfully set expected frequency for topic '" +
@@ -298,7 +370,7 @@ bool GreenwaveMonitor::add_topic(
     });
 
   greenwave_diagnostics::GreenwaveDiagnosticsConfig diagnostics_config;
-  diagnostics_config.enable_all_topic_diagnostics = true;
+  topic_has_header_[topic] = has_header_from_type(type);
 
   subscriptions_.push_back(sub);
   greenwave_diagnostics_.emplace(
@@ -330,6 +402,7 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes
   }
 
   greenwave_diagnostics_.erase(diag_it);
+  topic_has_header_.erase(topic);
   message = "Successfully removed topic";
   return true;
 }
@@ -444,7 +517,17 @@ void GreenwaveMonitor::add_topics_from_parameters()
     static const double retry_wait_s = 0.5;
     if (add_topic(topic, message, max_retries, retry_wait_s)) {
       if (expected_frequency > 0.0) {
-        greenwave_diagnostics_[topic]->setExpectedDt(expected_frequency, tolerance);
+        bool topic_has_header = false;
+        if (auto header_it = topic_has_header_.find(topic); header_it != topic_has_header_.end()) {
+          topic_has_header = header_it->second;
+        }
+        const auto [enable_node, enable_msg] = resolve_timestamp_checks(
+          timestamp_monitor_mode_, topic_has_header);
+        greenwave_diagnostics_[topic]->setExpectedDt(
+          expected_frequency,
+          tolerance,
+          enable_node,
+          enable_msg);
       } else {
         RCLCPP_WARN(
           this->get_logger(),

greenwave_monitor/src/minimal_publisher_node.cpp

@@ -83,7 +83,6 @@ MinimalPublisher::MinimalPublisher(const rclcpp::NodeOptions & options)
     std::chrono::nanoseconds(period_ns), std::bind(&MinimalPublisher::timer_callback, this));
 
   greenwave_diagnostics::GreenwaveDiagnosticsConfig diagnostics_config;
-  diagnostics_config.enable_all_topic_diagnostics = true;
   greenwave_diagnostics_ = std::make_unique<greenwave_diagnostics::GreenwaveDiagnostics>(
     *this, topic, diagnostics_config);
 }