update readme and fix tests

Author: sgillen

README.md

@@ -21,6 +21,21 @@ The diagnostics messages published by greenwave monitor are valid ROS 2 Diagnost
 
 In particular, the messages follow conventions from [Isaac ROS NITROS](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nitros), which means configured NITROS nodes can be monitored by greenwave monitor frontends without any additional subscriber overhead. For example the drivers from [Isaac ROS NOVA](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nova) can be monitored out of the box. Furthermore, you can set `ENABLE_GLOBAL_NITROS_DIAGNOSTICS=1` to configure all NITROS nodes to publish diagnostics (more info [here](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros/index.html)).
 
+  ## Latency Measurements
+
+  Latency is calculated as the difference between the current system time and the timestamp in the message header. For this calculation to work correctly:
+
+  - The message type must have a `std_msgs/Header` field
+  - The message type must be in the recognized types list (see `has_header_from_type()` in `greenwave_monitor.cpp`)
+  - The header timestamp must be in epoch time (not boottime)
+
+  If any of these conditions are not met, the latency will be reported as **"N/A"** in the dashboard. This typically occurs when:
+  - The message type doesn't have a header (e.g., `std_msgs/String`, `geometry_msgs/Twist`)
+  - The message type is not recognized by greenwave monitor
+  - The header timestamp is in boottime format instead of epoch time
+
+ Currently, message types with headers must be manually registered in the `known_header_types` map in `greenwave_monitor.cpp`. Support for automatic detection of arbitrary message types may be added in the future. In the meantime, if you need support for a commonly used message type, please submit an issue or pull request to add it to the registry.
+
 ## Compatibility
 
 Greenwave monitor is a standalone package tested on Humble, Iron, Jazzy, Kilted, and Rolling ROS 2 releases, under Ubuntu 22.04 and Ubuntu 24.04. It does not depend on Isaac ROS.

greenwave_monitor/greenwave_monitor/test_utils.py

@@ -17,6 +17,7 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
+import math
 import time
 from typing import List, Optional, Tuple
 
@@ -41,7 +42,6 @@
 MANAGE_TOPIC_TEST_CONFIG = TEST_CONFIGURATIONS[2]
 MONITOR_NODE_NAME = 'test_greenwave_monitor'
 MONITOR_NODE_NAMESPACE = 'test_namespace'
-ONE_YEAR_IN_MS = 365 * 24 * 60 * 60 * 1000
 
 
 def create_minimal_publisher(
@@ -178,7 +178,7 @@ def find_best_diagnostic(
     """Find the diagnostic message with frequency closest to expected."""
     best_status = None
     best_values = None
-    best_diff = float('inf')
+    best_diff = None
 
     for status in diagnostics:
         node_str = None
@@ -192,6 +192,8 @@ def find_best_diagnostic(
                 msg_str = kv.value
             elif kv.key == 'current_delay_from_realtime_ms':
                 latency_str = kv.value
+                if latency_str == 'N/A':
+                    latency_str = 'nan'
 
         try:
             node_val = float(node_str) if node_str is not None else None
@@ -242,9 +244,9 @@ def verify_diagnostic_values(status: DiagnosticStatus,
     if message_type == 'string':
         if reported_frequency_msg != 0.0:
             errors.append(f'String message frequency should be 0.0, got {reported_frequency_msg}')
-        if reported_latency_ms < ONE_YEAR_IN_MS:
+        if not math.isnan(reported_latency_ms):
             errors.append(
-                f'String latency should be >= {ONE_YEAR_IN_MS} ms, '
+                f'String latency should be {math.nan}, '
                 f'got {reported_latency_ms}')
     else:
         if abs(reported_frequency_msg - expected_frequency) > tolerance_hz:

greenwave_monitor/include/message_diagnostics.hpp

@@ -20,6 +20,7 @@
 #include <algorithm>
 #include <limits>
 #include <cinttypes>
+#include <cmath>
 #include <mutex>
 #include <queue>
 #include <string>
@@ -99,7 +100,7 @@ class MessageDiagnostics
     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;
-    message_latency_msg_ms_ = -1;
+    message_latency_msg_ms_ = std::numeric_limits<double>::quiet_NaN();
     outdated_msg_ = true;
 
     diagnostic_publisher_ =
@@ -161,7 +162,7 @@ class MessageDiagnostics
     // calculate key values for diagnostics status (computed on publish/getters)
     message_latency_msg_ms_ = latency_wrt_current_timestamp_node_ms;
     if (message_latency_msg_ms_ > message_diagnostics::constants::kNonsenseLatencyMs) {
-      message_latency_msg_ms_ = -1;
+      message_latency_msg_ms_ = std::numeric_limits<double>::quiet_NaN();
     }
 
     // frame dropping warnings
@@ -220,7 +221,7 @@ class MessageDiagnostics
       values.push_back(
         diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()
         .key("current_delay_from_realtime_ms")
-        .value(message_latency_msg_ms_ == -1 ?
+        .value(std::isnan(message_latency_msg_ms_) ?
           "N/A" : std::to_string(message_latency_msg_ms_)));
       values.push_back(
         diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()
@@ -280,7 +281,8 @@ class MessageDiagnostics
       RCLCPP_ERROR(
         node_.get_logger(),
         "expected_hz is 0.0. It should be set to a value greater than 0."
-        " Keeping previous values: expected_dt_us = %" PRId64 ", jitter_tolerance_us = %" PRId64 ".",
+        " Keeping previous values: expected_dt_us = %" PRId64 ","
+        " jitter_tolerance_us = %" PRId64 ".",
         static_cast<int64_t>(diagnostics_config_.expected_dt_us),
         diagnostics_config_.jitter_tolerance_us);
       return;
@@ -290,8 +292,8 @@ class MessageDiagnostics
       static_cast<int64_t>(message_diagnostics::constants::kSecondsToMicroseconds / expected_hz);
     diagnostics_config_.expected_dt_us = expected_dt_us;
 
-    const int64_t tolerance_us =
-      static_cast<int64_t>((message_diagnostics::constants::kSecondsToMicroseconds / expected_hz) *
+    const int tolerance_us =
+      static_cast<int>((message_diagnostics::constants::kSecondsToMicroseconds / expected_hz) *
       (tolerance_percent / 100.0));
     diagnostics_config_.jitter_tolerance_us = tolerance_us;
   }
@@ -302,18 +304,18 @@ class MessageDiagnostics
     diagnostics_config_.enable_node_time_diagnostics = false;
     diagnostics_config_.enable_msg_time_diagnostics = false;
 
-    diagnostics_config_.expected_dt_us = 0LL;
-    diagnostics_config_.jitter_tolerance_us = 0LL;
+    diagnostics_config_.expected_dt_us = 0;
+    diagnostics_config_.jitter_tolerance_us = 0;
   }
 
 private:
   struct RollingWindow
   {
     int window_size{0};
     std::queue<int64_t> interarrival_us;
-    int64_t sum_interarrival_us{0LL};
+    int64_t sum_interarrival_us{0};
     std::queue<int64_t> jitter_abs_us;
-    int64_t sum_jitter_abs_us{0LL};
+    int64_t sum_jitter_abs_us{0};
     int64_t max_abs_jitter_us{0};
     uint64_t outlier_count{0};
 
@@ -357,7 +359,7 @@ class MessageDiagnostics
     int64_t meanAbsJitterUs() const
     {
       if (jitter_abs_us.empty()) {
-        return 0LL;
+        return 0;
       }
       return sum_jitter_abs_us / static_cast<int64_t>(jitter_abs_us.size());
     }

scripts/docker-test.sh

@@ -53,7 +53,7 @@ echo "Starting Docker container for ROS 2 $DISTRO..."
 echo "Image: $IMAGE"
 
 # Get current directory (should be the greenwave_monitor root)
-WORKSPACE_DIR=$(pwd)/../
+WORKSPACE_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)/../"
 
 # Run container with interactive shell, mounting current directory
 docker run -it --rm \
@@ -73,9 +73,9 @@ docker run -it --rm \
 
         # Install Python requirements based on ROS distro
         if [[ '${DISTRO}' == 'jazzy' || '${DISTRO}' == 'kilted' || '${DISTRO}' == 'rolling' ]]; then
-            pip3 install --break-system-packages -I pygments -r src/greenwave_monitor/requirements.txt
+            pip3 install --break-system-packages -I pygments -r /workspace/src/greenwave_monitor/r2s_gw/requirements.txt
         else
-            pip3 install -r src/greenwave_monitor/requirements.txt
+            pip3 install -r /workspace/src/greenwave_monitor/r2s_gw/requirements.txt
         fi
 
         echo '=== Building packages ==='