From e4128920ef4627f000ba20e1ead010260ba8472c Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Thu, 26 Feb 2026 14:03:26 -0800 Subject: [PATCH] Rename msg time references to hdr time Replace all "message time" / "msg" identifiers that refer to header-based timestamps with "header time" / "hdr" equivalents across code, tests, Python utilities, and documentation. Updates struct fields, function names, diagnostic key strings, local variables, and comments consistently. Signed-off-by: bmchale Co-Authored-By: Claude Sonnet 4.6 --- docs/DESIGN_AND_IMPLEMENTATION.md | 12 +- .../greenwave_monitor/test_utils.py | 4 +- .../greenwave_monitor/ui_adaptor.py | 2 +- .../include/greenwave_diagnostics.hpp | 130 ++++++++-------- greenwave_monitor/src/greenwave_monitor.cpp | 6 +- .../src/minimal_publisher_node.cpp | 4 +- .../test/test_greenwave_diagnostics.cpp | 144 +++++++++--------- .../test/test_topic_monitoring_integration.py | 2 +- 8 files changed, 152 insertions(+), 152 deletions(-) diff --git a/docs/DESIGN_AND_IMPLEMENTATION.md b/docs/DESIGN_AND_IMPLEMENTATION.md index 4bd4655..68830ef 100644 --- a/docs/DESIGN_AND_IMPLEMENTATION.md +++ b/docs/DESIGN_AND_IMPLEMENTATION.md @@ -23,7 +23,7 @@ The diagnostics messages published by greenwave monitor are standard ROS 2 Diagn `GreenwaveDiagnostics` tracks: - node-time interarrival rate (`frame_rate_node`) -- message-time interarrival rate (`frame_rate_msg`) # Using the message's header timestamp +- header-time interarrival rate (`frame_rate_hdr`) # Using the message's header timestamp - current delay from realtime (`current_delay_from_realtime_ms`) - jitter/outlier counters and summary stats - status transitions (`OK`, `ERROR`, `STALE`) for missed timing expectations @@ -87,7 +87,7 @@ gw_diag_->setExpectedDt(/*expected_hz=*/30.0, /*tolerance_percent=*/10.0); ### Step 3: Update diagnostics per message event -Call `updateDiagnostics(msg_timestamp_ns)` whenever you publish or consume. +Call `updateDiagnostics(hdr_timestamp_ns)` whenever you publish or consume. ```cpp const auto stamp_ns = msg.header.stamp.sec * 1000000000ULL + msg.header.stamp.nanosec; @@ -116,7 +116,7 @@ diag_timer_ = this->create_wall_timer( Dashboards expect specific keys inside `DiagnosticStatus.values`, including: - `frame_rate_node` -- `frame_rate_msg` +- `frame_rate_hdr` - `current_delay_from_realtime_ms` - `expected_frequency` - `tolerance` @@ -138,8 +138,8 @@ The `gw_time_check_preset` node parameter controls which time-based checks run o | Preset value | Description | |--------------|-------------| -| `header_with_nodetime_fallback` (default) | For header-bearing types: check message timestamp (rate, jitter, increasing). For headerless types: fall back to node time and run FPS window and increasing-timestamp checks. | -| `header_only` | Check message timestamp only. Headerless topics get no timing checks (diagnostics stay OK). Use when you only care about headered sources. | +| `header_with_nodetime_fallback` (default) | For header-bearing types: check header timestamp (rate, jitter, increasing). For headerless types: fall back to node time and run FPS window and increasing-timestamp checks. | +| `header_only` | Check header timestamp only. Headerless topics get no timing checks (diagnostics stay OK). Use when you only care about headered sources. | | `nodetime_only` | Use node receive time for all topics (header and headerless). Runs FPS window and increasing-timestamp checks. | | `none` | No time-based checks; only raw rate/latency values are computed and published. | @@ -156,7 +156,7 @@ Invalid values are ignored and the default `header_with_nodetime_fallback` is us ## Implementation Notes And Pitfalls -- Message timestamp should be epoch time for latency to be meaningful. +- Header timestamp should be epoch time for latency to be meaningful. - The central monitor only parses headers for types listed in `GreenwaveMonitor::has_header_from_type()`; unknown types fall back to no-header behavior. - `publishDiagnostics()` marks status as `STALE` if no fresh `updateDiagnostics()` happened since the previous publish. diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index e952720..56a9cbd 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -190,7 +190,7 @@ def find_best_diagnostic( for kv in status.values: if kv.key == 'frame_rate_node': node_str = kv.value - elif kv.key == 'frame_rate_msg': + elif kv.key == 'frame_rate_hdr': msg_str = kv.value elif kv.key == 'current_delay_from_realtime_ms': latency_str = kv.value @@ -233,7 +233,7 @@ def verify_diagnostic_values(status: DiagnosticStatus, if reported_frequency_node == -1.0: errors.append("Did not find 'frame_rate_node' in diagnostic") if reported_frequency_msg == -1.0: - errors.append("Did not find 'frame_rate_msg' in diagnostic") + errors.append("Did not find 'frame_rate_hdr' in diagnostic") if reported_latency_ms == -1.0: errors.append("Did not find 'current_delay_from_realtime_ms' in diagnostic") diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 015962b..b43dcc1 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -83,7 +83,7 @@ def from_status(cls, status: DiagnosticStatus) -> 'UiDiagnosticData': for kv in status.values: if kv.key == 'frame_rate_node': data.pub_rate = kv.value - elif kv.key == 'frame_rate_msg': + elif kv.key == 'frame_rate_hdr': data.msg_rate = kv.value elif kv.key == 'current_delay_from_realtime_ms': data.latency = kv.value diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index f66d37a..93e6696 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -64,19 +64,19 @@ struct GreenwaveDiagnosticsConfig // flags for enabling/disabling specific diagnostics bool enable_node_time_diagnostics{false}; - bool enable_msg_time_diagnostics{false}; - bool enable_fps_jitter_msg_time_diagnostics{false}; + bool enable_hdr_time_diagnostics{false}; + bool enable_fps_jitter_hdr_time_diagnostics{false}; bool enable_fps_jitter_node_time_diagnostics{false}; - bool enable_fps_window_msg_time_diagnostics{false}; + bool enable_fps_window_hdr_time_diagnostics{false}; bool enable_fps_window_node_time_diagnostics{false}; - bool enable_increasing_msg_time_diagnostics{false}; + bool enable_increasing_hdr_time_diagnostics{false}; bool enable_increasing_node_time_diagnostics{false}; - // fallback to node time when message time is not available + // fallback to node time when header time is not available bool fallback_to_nodetime{false}; - // flag indicating if it's expected for the message to have a timestamp - bool has_msg_timestamp{false}; + // flag indicating if it's expected for the message to have a header timestamp + bool has_hdr_timestamp{false}; // enable basic diagnostics for all topics, triggered by an environment variable bool enable_all_topic_diagnostics{false}; @@ -100,34 +100,34 @@ struct GreenwaveDiagnosticsConfig enable_fps_jitter_node_time_diagnostics = false; enable_fps_window_node_time_diagnostics = false; enable_increasing_node_time_diagnostics = false; - enable_msg_time_diagnostics = true; - enable_fps_jitter_msg_time_diagnostics = true; - enable_fps_window_msg_time_diagnostics = false; - enable_increasing_msg_time_diagnostics = true; + enable_hdr_time_diagnostics = true; + enable_fps_jitter_hdr_time_diagnostics = true; + enable_fps_window_hdr_time_diagnostics = false; + enable_increasing_hdr_time_diagnostics = true; break; case TimeCheckPreset::NodetimeOnly: enable_node_time_diagnostics = true; enable_fps_jitter_node_time_diagnostics = false; enable_fps_window_node_time_diagnostics = true; enable_increasing_node_time_diagnostics = true; - enable_msg_time_diagnostics = false; - enable_fps_jitter_msg_time_diagnostics = false; - enable_fps_window_msg_time_diagnostics = false; - enable_increasing_msg_time_diagnostics = false; + enable_hdr_time_diagnostics = false; + enable_fps_jitter_hdr_time_diagnostics = false; + enable_fps_window_hdr_time_diagnostics = false; + enable_increasing_hdr_time_diagnostics = false; break; case TimeCheckPreset::HeaderWithFallback: - // Enable msg time diagnostics only when it is known there is a msg timestamp, otherwise + // Enable hdr time diagnostics only when it is known there is a hdr timestamp, otherwise // disable them and use node time only. fps jitter checks for node time are too restrictive, // disable them. fallback_to_nodetime = true; enable_node_time_diagnostics = true; enable_fps_jitter_node_time_diagnostics = false; - enable_fps_window_node_time_diagnostics = !has_msg_timestamp; + enable_fps_window_node_time_diagnostics = !has_hdr_timestamp; enable_increasing_node_time_diagnostics = true; - enable_msg_time_diagnostics = has_msg_timestamp; - enable_fps_jitter_msg_time_diagnostics = has_msg_timestamp; - enable_fps_window_msg_time_diagnostics = false; - enable_increasing_msg_time_diagnostics = has_msg_timestamp; + enable_hdr_time_diagnostics = has_hdr_timestamp; + enable_fps_jitter_hdr_time_diagnostics = has_hdr_timestamp; + enable_fps_window_hdr_time_diagnostics = false; + enable_increasing_hdr_time_diagnostics = has_hdr_timestamp; break; } } @@ -158,18 +158,18 @@ class GreenwaveDiagnostics node_ts_series_.increasing_error_message = "NONINCREASING TIMESTAMP (NODE TIME)"; node_ts_series_.fps_window_error_message = "FPS OUT OF RANGE (NODE TIME)"; - msg_ts_series_.label = "Message Time"; - msg_ts_series_.enabled = diagnostics_config_.enable_msg_time_diagnostics; - msg_ts_series_.check_fps_jitter = diagnostics_config_.enable_fps_jitter_msg_time_diagnostics; - msg_ts_series_.check_fps_window = diagnostics_config_.enable_fps_window_msg_time_diagnostics; - msg_ts_series_.check_increasing = diagnostics_config_.enable_increasing_msg_time_diagnostics; - msg_ts_series_.window.window_size = diagnostics_config_.filter_window_size; - msg_ts_series_.prev_drop_ts = rclcpp::Time(0, 0, clock_->get_clock_type()); - msg_ts_series_.prev_noninc_ts = rclcpp::Time(0, 0, clock_->get_clock_type()); - msg_ts_series_.prev_fps_out_of_range_ts = rclcpp::Time(0, 0, clock_->get_clock_type()); - msg_ts_series_.drop_error_message = "FRAME DROP DETECTED"; - msg_ts_series_.increasing_error_message = "NONINCREASING TIMESTAMP"; - msg_ts_series_.fps_window_error_message = "FPS OUT OF RANGE"; + hdr_ts_series_.label = "Header Time"; + hdr_ts_series_.enabled = diagnostics_config_.enable_hdr_time_diagnostics; + hdr_ts_series_.check_fps_jitter = diagnostics_config_.enable_fps_jitter_hdr_time_diagnostics; + hdr_ts_series_.check_fps_window = diagnostics_config_.enable_fps_window_hdr_time_diagnostics; + hdr_ts_series_.check_increasing = diagnostics_config_.enable_increasing_hdr_time_diagnostics; + hdr_ts_series_.window.window_size = diagnostics_config_.filter_window_size; + hdr_ts_series_.prev_drop_ts = rclcpp::Time(0, 0, clock_->get_clock_type()); + hdr_ts_series_.prev_noninc_ts = rclcpp::Time(0, 0, clock_->get_clock_type()); + hdr_ts_series_.prev_fps_out_of_range_ts = rclcpp::Time(0, 0, clock_->get_clock_type()); + hdr_ts_series_.drop_error_message = "FRAME DROP DETECTED"; + hdr_ts_series_.increasing_error_message = "NONINCREASING TIMESTAMP"; + hdr_ts_series_.fps_window_error_message = "FPS OUT OF RANGE"; diagnostic_msgs::msg::DiagnosticStatus topic_status; topic_status.name = topic_name; @@ -186,34 +186,34 @@ class GreenwaveDiagnostics } // Update diagnostics numbers. To be called in Subscriber and Publisher - void updateDiagnostics(uint64_t msg_timestamp_ns) + void updateDiagnostics(uint64_t hdr_timestamp_ns) { const std::lock_guard lock(greenwave_diagnostics_mutex_); status_vec_[0].message = ""; bool error_found = false; - const uint64_t current_timestamp_msg_us = diagnostics_config_.has_msg_timestamp ? - msg_timestamp_ns / constants::kMicrosecondsToNanoseconds : 0; + const uint64_t current_timestamp_hdr_us = diagnostics_config_.has_hdr_timestamp ? + hdr_timestamp_ns / constants::kMicrosecondsToNanoseconds : 0; const uint64_t current_timestamp_node_us = static_cast( clock_->now().nanoseconds() / constants::kMicrosecondsToNanoseconds); // Node time source error_found |= updateTimeSource(node_ts_series_, current_timestamp_node_us); - // Message time source (header-bearing messages only) - if (diagnostics_config_.has_msg_timestamp) { - error_found |= updateTimeSource(msg_ts_series_, current_timestamp_msg_us); + // Header time source (header-bearing messages only) + if (diagnostics_config_.has_hdr_timestamp) { + error_found |= updateTimeSource(hdr_ts_series_, current_timestamp_hdr_us); } - // Latency (only valid if message time source is available) - if (diagnostics_config_.has_msg_timestamp) { + // Latency (only valid if header time source is available) + if (diagnostics_config_.has_hdr_timestamp) { const uint64_t ros_node_system_time_us = static_cast( clock_->now().nanoseconds() / constants::kMicrosecondsToNanoseconds); - message_latency_msg_ms_ = - static_cast(ros_node_system_time_us - current_timestamp_msg_us) / + message_latency_hdr_ms_ = + static_cast(ros_node_system_time_us - current_timestamp_hdr_us) / constants::kMillisecondsToMicroseconds; - if (message_latency_msg_ms_ > constants::kNonsenseLatencyMs) { - message_latency_msg_ms_ = std::numeric_limits::quiet_NaN(); + if (message_latency_hdr_ms_ > constants::kNonsenseLatencyMs) { + message_latency_hdr_ms_ = std::numeric_limits::quiet_NaN(); } } @@ -239,42 +239,42 @@ class GreenwaveDiagnostics } else { values.push_back( diagnostic_msgs::build() - .key("num_non_increasing_msg") - .value(std::to_string(msg_ts_series_.num_non_increasing))); + .key("num_non_increasing_hdr") + .value(std::to_string(hdr_ts_series_.num_non_increasing))); values.push_back( diagnostic_msgs::build() - .key("num_jitter_outliers_msg") - .value(std::to_string(msg_ts_series_.window.outlier_count))); + .key("num_jitter_outliers_hdr") + .value(std::to_string(hdr_ts_series_.window.outlier_count))); values.push_back( diagnostic_msgs::build() .key("num_jitter_outliers_node") .value(std::to_string(node_ts_series_.window.outlier_count))); values.push_back( diagnostic_msgs::build() - .key("max_abs_jitter_msg") - .value(std::to_string(msg_ts_series_.window.max_abs_jitter_us))); + .key("max_abs_jitter_hdr") + .value(std::to_string(hdr_ts_series_.window.max_abs_jitter_us))); values.push_back( diagnostic_msgs::build() .key("max_abs_jitter_node") .value(std::to_string(node_ts_series_.window.max_abs_jitter_us))); values.push_back( diagnostic_msgs::build() - .key("mean_abs_jitter_msg") - .value(std::to_string(msg_ts_series_.window.meanAbsJitterUs()))); + .key("mean_abs_jitter_hdr") + .value(std::to_string(hdr_ts_series_.window.meanAbsJitterUs()))); values.push_back( diagnostic_msgs::build() .key("mean_abs_jitter_node") .value(std::to_string(node_ts_series_.window.meanAbsJitterUs()))); values.push_back( diagnostic_msgs::build() - .key("frame_rate_msg") - .value(std::to_string(msg_ts_series_.window.frameRateHz()))); + .key("frame_rate_hdr") + .value(std::to_string(hdr_ts_series_.window.frameRateHz()))); values.push_back( diagnostic_msgs::build() .key("current_delay_from_realtime_ms") .value( - std::isnan(message_latency_msg_ms_) ? - "N/A" : std::to_string(message_latency_msg_ms_))); + std::isnan(message_latency_hdr_ms_) ? + "N/A" : std::to_string(message_latency_hdr_ms_))); values.push_back( diagnostic_msgs::build() .key("frame_rate_node") @@ -282,7 +282,7 @@ class GreenwaveDiagnostics values.push_back( diagnostic_msgs::build() .key("total_dropped_frames") - .value(std::to_string(msg_ts_series_.window.outlier_count))); + .value(std::to_string(hdr_ts_series_.window.outlier_count))); values.push_back( diagnostic_msgs::build() .key("expected_frequency") @@ -319,21 +319,21 @@ class GreenwaveDiagnostics return node_ts_series_.window.frameRateHz(); } - double getFrameRateMsg() const + double getFrameRateHdr() const { - return msg_ts_series_.window.frameRateHz(); + return hdr_ts_series_.window.frameRateHz(); } double getLatency() const { - return message_latency_msg_ms_; + return message_latency_hdr_ms_; } void setExpectedDt(double expected_hz, double tolerance_percent) { const std::lock_guard lock(greenwave_diagnostics_mutex_); node_ts_series_.enabled = true; - msg_ts_series_.enabled = true; + hdr_ts_series_.enabled = true; // This prevents accidental 0 division in the calculations in case of // a direct function call (not from service in greenwave_monitor.cpp) if (expected_hz == 0.0) { @@ -363,7 +363,7 @@ class GreenwaveDiagnostics { const std::lock_guard lock(greenwave_diagnostics_mutex_); node_ts_series_.enabled = false; - msg_ts_series_.enabled = false; + hdr_ts_series_.enabled = false; diagnostics_config_.expected_dt_us = 0; diagnostics_config_.jitter_tolerance_us = 0; @@ -464,9 +464,9 @@ class GreenwaveDiagnostics rclcpp::Time t_start_; TimeSeriesState node_ts_series_; - TimeSeriesState msg_ts_series_; + TimeSeriesState hdr_ts_series_; - double message_latency_msg_ms_{std::numeric_limits::quiet_NaN()}; + double message_latency_hdr_ms_{std::numeric_limits::quiet_NaN()}; bool outdated_msg_{true}; rclcpp::Publisher::SharedPtr diagnostic_publisher_; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 48b26ef..004d1e9 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -148,8 +148,8 @@ void GreenwaveMonitor::topic_callback( const std::shared_ptr msg, const std::string & topic, const std::string & type) { - auto msg_timestamp = GetTimestampFromSerializedMessage(msg, type); - greenwave_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); + auto hdr_timestamp = GetTimestampFromSerializedMessage(msg, type); + greenwave_diagnostics_[topic]->updateDiagnostics(hdr_timestamp.time_since_epoch().count()); } void GreenwaveMonitor::timer_callback() @@ -347,7 +347,7 @@ bool GreenwaveMonitor::add_topic( greenwave_diagnostics::GreenwaveDiagnosticsConfig diagnostics_config; diagnostics_config.enable_all_topic_diagnostics = true; diagnostics_config.time_check_preset = time_check_preset_; - diagnostics_config.has_msg_timestamp = has_header_from_type(type); + diagnostics_config.has_hdr_timestamp = has_header_from_type(type); subscriptions_.push_back(sub); greenwave_diagnostics_.emplace( diff --git a/greenwave_monitor/src/minimal_publisher_node.cpp b/greenwave_monitor/src/minimal_publisher_node.cpp index 88b695b..6b23a45 100644 --- a/greenwave_monitor/src/minimal_publisher_node.cpp +++ b/greenwave_monitor/src/minimal_publisher_node.cpp @@ -137,7 +137,7 @@ void MinimalPublisher::timer_callback() message); } - const auto msg_timestamp = this->now(); - greenwave_diagnostics_->updateDiagnostics(msg_timestamp.nanoseconds()); + const auto hdr_timestamp = this->now(); + greenwave_diagnostics_->updateDiagnostics(hdr_timestamp.nanoseconds()); // greenwave_diagnostics_->publishDiagnostics(); } diff --git a/greenwave_monitor/test/test_greenwave_diagnostics.cpp b/greenwave_monitor/test/test_greenwave_diagnostics.cpp index 7367f9c..2533123 100644 --- a/greenwave_monitor/test/test_greenwave_diagnostics.cpp +++ b/greenwave_monitor/test/test_greenwave_diagnostics.cpp @@ -62,10 +62,10 @@ class GreenwaveDiagnosticsTest : public ::testing::Test std::shared_ptr node_; }; -TEST_F(GreenwaveDiagnosticsTest, FrameRateMsgTest) +TEST_F(GreenwaveDiagnosticsTest, FrameRateHdrTest) { greenwave_diagnostics::GreenwaveDiagnosticsConfig config; - config.has_msg_timestamp = true; + config.has_hdr_timestamp = true; greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( *node_, "test_topic", config); @@ -74,7 +74,7 @@ TEST_F(GreenwaveDiagnosticsTest, FrameRateMsgTest) greenwave_diagnostics.updateDiagnostics(timestamp); timestamp += 10000000; // 10 ms in nanoseconds } - EXPECT_EQ(greenwave_diagnostics.getFrameRateMsg(), 100); // 100 Hz + EXPECT_EQ(greenwave_diagnostics.getFrameRateHdr(), 100); // 100 Hz } TEST_F(GreenwaveDiagnosticsTest, FrameRateNodeTest) @@ -108,18 +108,18 @@ TEST_F(GreenwaveDiagnosticsTest, MessageLatencyTest) { greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::HeaderOnly; - config.has_msg_timestamp = true; + config.has_hdr_timestamp = true; greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( *node_, "test_topic", config); const rclcpp::Time current_time = node_->get_clock()->now(); - // Make message timestamp a certain amount of time earlier than current time + // Make header timestamp a certain amount of time earlier than current time constexpr double expected_latency_ms = 10.0; - const rclcpp::Time msg_timestamp = + const rclcpp::Time hdr_timestamp = current_time - rclcpp::Duration::from_seconds( expected_latency_ms / static_cast(test_constants::kMillisecondsToSeconds)); - greenwave_diagnostics.updateDiagnostics(msg_timestamp.nanoseconds()); + greenwave_diagnostics.updateDiagnostics(hdr_timestamp.nanoseconds()); // allow 1 ms tolerance EXPECT_NEAR(greenwave_diagnostics.getLatency(), expected_latency_ms, 1.0); @@ -133,11 +133,11 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) ::greenwave_diagnostics::constants::kSecondsToNanoseconds / input_frequency); greenwave_diagnostics::GreenwaveDiagnosticsConfig config; - config.enable_msg_time_diagnostics = true; + config.enable_hdr_time_diagnostics = true; config.enable_node_time_diagnostics = true; - config.enable_fps_jitter_msg_time_diagnostics = true; - config.enable_increasing_msg_time_diagnostics = true; - config.has_msg_timestamp = true; + config.enable_fps_jitter_hdr_time_diagnostics = true; + config.enable_increasing_hdr_time_diagnostics = true; + config.has_hdr_timestamp = true; config.expected_dt_us = interarrival_time_ns / ::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; @@ -156,8 +156,8 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) constexpr int64_t delay_time_ns = 50 * static_cast(::greenwave_diagnostics::constants::kMillisecondsToMicroseconds) * static_cast(::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds); - // Starting message timestamp in nanoseconds - auto msg_timestamp = test_constants::kStartTimestampNs; + // Starting header timestamp in nanoseconds + auto hdr_timestamp = test_constants::kStartTimestampNs; int sent_count = 0; const auto start_time = std::chrono::high_resolution_clock::now(); @@ -166,22 +166,22 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) constexpr int num_messages = 100; while (sent_count < num_messages) { if (sent_count != 0) { - msg_timestamp += interarrival_time_ns; + hdr_timestamp += interarrival_time_ns; } sent_count++; - greenwave_diagnostics.updateDiagnostics(msg_timestamp); + greenwave_diagnostics.updateDiagnostics(hdr_timestamp); greenwave_diagnostics.publishDiagnostics(); // Add a non-increasing timestamp at count 5 if (sent_count == 5) { - msg_timestamp -= interarrival_time_ns; + hdr_timestamp -= interarrival_time_ns; } // Add a jitter by delaying at count 10 if (sent_count == 10) { std::this_thread::sleep_for(std::chrono::nanoseconds(delay_time_ns)); // 50 ms delay - msg_timestamp += delay_time_ns; + hdr_timestamp += delay_time_ns; } rclcpp::spin_some(node_); @@ -198,11 +198,11 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) const double expected_frame_rate_node = static_cast(interarrival_time_count) / total_duration.count(); - const auto sum_interarrival_time_msg_sec = static_cast( - msg_timestamp - test_constants::kStartTimestampNs) / + const auto sum_interarrival_time_hdr_sec = static_cast( + hdr_timestamp - test_constants::kStartTimestampNs) / static_cast(::greenwave_diagnostics::constants::kSecondsToNanoseconds); - const double expected_frame_rate_msg = - static_cast(interarrival_time_count) / sum_interarrival_time_msg_sec; + const double expected_frame_rate_hdr = + static_cast(interarrival_time_count) / sum_interarrival_time_hdr_sec; // Verify that we received diagnostic messages ASSERT_FALSE(received_diagnostics.empty()); @@ -220,14 +220,14 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) // Parse diagnostic values std::map diagnostics_values = { {"frame_rate_node", 0.0}, - {"num_non_increasing_msg", 0.0}, - {"num_jitter_outliers_msg", 0.0}, + {"num_non_increasing_hdr", 0.0}, + {"num_jitter_outliers_hdr", 0.0}, {"num_jitter_outliers_node", 0.0}, - {"max_abs_jitter_msg", 0.0}, + {"max_abs_jitter_hdr", 0.0}, {"max_abs_jitter_node", 0.0}, - {"mean_abs_jitter_msg", 0.0}, + {"mean_abs_jitter_hdr", 0.0}, {"mean_abs_jitter_node", 0.0}, - {"frame_rate_msg", 0.0}, + {"frame_rate_hdr", 0.0}, {"total_dropped_frames", 0.0} }; for (const auto & value : diagnostic_status.values) { @@ -239,32 +239,32 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) // Sometimes diagnostics may arrive out of order, so we use getter methods instead of values from // the last diagnostic message to prevent flakiness EXPECT_NEAR(greenwave_diagnostics.getFrameRateNode(), expected_frame_rate_node, 1.0); - // Allow small floating point differences for frame rate msg - constexpr double frame_rate_msg_tolerance = 0.001; + // Allow small floating point differences for frame rate hdr + constexpr double frame_rate_hdr_tolerance = 0.001; EXPECT_NEAR( - greenwave_diagnostics.getFrameRateMsg(), expected_frame_rate_msg, frame_rate_msg_tolerance); + greenwave_diagnostics.getFrameRateHdr(), expected_frame_rate_hdr, frame_rate_hdr_tolerance); // Sometimes diagnostics may arrive out of order, so we need to check all received diagnostics - // to see if the expected msg frame rate is somewhere in there - double smallest_msg_frame_rate_diff = std::numeric_limits::infinity(); + // to see if the expected hdr frame rate is somewhere in there + double smallest_hdr_frame_rate_diff = std::numeric_limits::infinity(); for (const auto & diag_msg : received_diagnostics) { if (diag_msg->status.empty()) { continue; } const auto & status = diag_msg->status[0]; - double frame_rate_msg = 0.0; + double frame_rate_hdr = 0.0; for (const auto & value : status.values) { - if (value.key == "frame_rate_msg") { - frame_rate_msg = std::stod(value.value); + if (value.key == "frame_rate_hdr") { + frame_rate_hdr = std::stod(value.value); break; } } - if (std::abs(frame_rate_msg - expected_frame_rate_msg) < smallest_msg_frame_rate_diff) { - smallest_msg_frame_rate_diff = std::abs(frame_rate_msg - expected_frame_rate_msg); + if (std::abs(frame_rate_hdr - expected_frame_rate_hdr) < smallest_hdr_frame_rate_diff) { + smallest_hdr_frame_rate_diff = std::abs(frame_rate_hdr - expected_frame_rate_hdr); } } - EXPECT_LT(smallest_msg_frame_rate_diff, frame_rate_msg_tolerance); + EXPECT_LT(smallest_hdr_frame_rate_diff, frame_rate_hdr_tolerance); // Diagnostics should have at least one jitter due to the intentional delay // possibly more if the system was very busy @@ -272,12 +272,12 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) EXPECT_GE(diagnostics_values["max_abs_jitter_node"], 0.0); EXPECT_GE(diagnostics_values["mean_abs_jitter_node"], 0.0); - EXPECT_GE(diagnostics_values["num_jitter_outliers_msg"], 1.0); - EXPECT_GE(diagnostics_values["max_abs_jitter_msg"], 0.0); - EXPECT_GE(diagnostics_values["mean_abs_jitter_msg"], 0.0); + EXPECT_GE(diagnostics_values["num_jitter_outliers_hdr"], 1.0); + EXPECT_GE(diagnostics_values["max_abs_jitter_hdr"], 0.0); + EXPECT_GE(diagnostics_values["mean_abs_jitter_hdr"], 0.0); EXPECT_GE(diagnostics_values["total_dropped_frames"], 1.0); - EXPECT_GE(diagnostics_values["num_non_increasing_msg"], 1.0); + EXPECT_GE(diagnostics_values["num_non_increasing_hdr"], 1.0); } static diagnostic_msgs::msg::DiagnosticStatus run_fps_sequence( @@ -302,12 +302,12 @@ static diagnostic_msgs::msg::DiagnosticStatus run_fps_sequence( const int64_t input_dt_ns = static_cast( 1e9 / expected_hz); const auto sleep_duration = std::chrono::duration(1.0 / actual_hz); - uint64_t msg_timestamp = test_constants::kStartTimestampNs; + uint64_t hdr_timestamp = test_constants::kStartTimestampNs; for (int i = 0; i < samples; ++i) { - diagnostics.updateDiagnostics(msg_timestamp); + diagnostics.updateDiagnostics(hdr_timestamp); diagnostics.publishDiagnostics(); rclcpp::spin_some(node); - msg_timestamp += static_cast(input_dt_ns); + hdr_timestamp += static_cast(input_dt_ns); std::this_thread::sleep_for(sleep_duration); } @@ -324,7 +324,7 @@ TEST_F(GreenwaveDiagnosticsTest, HeaderlessFallbackRaisesOutOfRangeFpsError) { greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::HeaderWithFallback; - config.has_msg_timestamp = false; + config.has_hdr_timestamp = false; const auto status = run_fps_sequence(node_, config, 100.0, 10.0, 40.0); @@ -336,7 +336,7 @@ TEST_F(GreenwaveDiagnosticsTest, HeaderOnlyDoesNotUseFallbackForHeaderlessTopics { greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::HeaderOnly; - config.has_msg_timestamp = false; + config.has_hdr_timestamp = false; const auto status = run_fps_sequence(node_, config, 100.0, 10.0, 40.0); @@ -348,7 +348,7 @@ TEST_F(GreenwaveDiagnosticsTest, FallbackModeDoesNotUseFpsWindowForHeaderedTopic { greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::HeaderWithFallback; - config.has_msg_timestamp = true; + config.has_hdr_timestamp = true; const auto status = run_fps_sequence(node_, config, 100.0, 10.0, 100.0); @@ -360,7 +360,7 @@ TEST_F(GreenwaveDiagnosticsTest, NodetimeOnlyUsesFpsWindowForHeaderedTopics) { greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::NodetimeOnly; - config.has_msg_timestamp = true; + config.has_hdr_timestamp = true; const auto status = run_fps_sequence(node_, config, 100.0, 10.0, 40.0); @@ -374,12 +374,12 @@ TEST_F(GreenwaveDiagnosticsTest, TimeCheckPresetNone) config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::None; config.applyTimeCheckPreset(); EXPECT_FALSE(config.enable_node_time_diagnostics); - EXPECT_FALSE(config.enable_msg_time_diagnostics); - EXPECT_FALSE(config.enable_fps_jitter_msg_time_diagnostics); + EXPECT_FALSE(config.enable_hdr_time_diagnostics); + EXPECT_FALSE(config.enable_fps_jitter_hdr_time_diagnostics); EXPECT_FALSE(config.enable_fps_jitter_node_time_diagnostics); - EXPECT_FALSE(config.enable_fps_window_msg_time_diagnostics); + EXPECT_FALSE(config.enable_fps_window_hdr_time_diagnostics); EXPECT_FALSE(config.enable_fps_window_node_time_diagnostics); - EXPECT_FALSE(config.enable_increasing_msg_time_diagnostics); + EXPECT_FALSE(config.enable_increasing_hdr_time_diagnostics); EXPECT_FALSE(config.enable_increasing_node_time_diagnostics); EXPECT_FALSE(config.fallback_to_nodetime); } @@ -390,12 +390,12 @@ TEST_F(GreenwaveDiagnosticsTest, TimeCheckPresetHeaderOnly) config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::HeaderOnly; config.applyTimeCheckPreset(); EXPECT_FALSE(config.enable_node_time_diagnostics); - EXPECT_TRUE(config.enable_msg_time_diagnostics); - EXPECT_TRUE(config.enable_fps_jitter_msg_time_diagnostics); + EXPECT_TRUE(config.enable_hdr_time_diagnostics); + EXPECT_TRUE(config.enable_fps_jitter_hdr_time_diagnostics); EXPECT_FALSE(config.enable_fps_jitter_node_time_diagnostics); - EXPECT_FALSE(config.enable_fps_window_msg_time_diagnostics); + EXPECT_FALSE(config.enable_fps_window_hdr_time_diagnostics); EXPECT_FALSE(config.enable_fps_window_node_time_diagnostics); - EXPECT_TRUE(config.enable_increasing_msg_time_diagnostics); + EXPECT_TRUE(config.enable_increasing_hdr_time_diagnostics); EXPECT_FALSE(config.enable_increasing_node_time_diagnostics); EXPECT_FALSE(config.fallback_to_nodetime); } @@ -406,46 +406,46 @@ TEST_F(GreenwaveDiagnosticsTest, TimeCheckPresetNodetimeOnly) config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::NodetimeOnly; config.applyTimeCheckPreset(); EXPECT_TRUE(config.enable_node_time_diagnostics); - EXPECT_FALSE(config.enable_msg_time_diagnostics); - EXPECT_FALSE(config.enable_fps_jitter_msg_time_diagnostics); + EXPECT_FALSE(config.enable_hdr_time_diagnostics); + EXPECT_FALSE(config.enable_fps_jitter_hdr_time_diagnostics); EXPECT_FALSE(config.enable_fps_jitter_node_time_diagnostics); - EXPECT_FALSE(config.enable_fps_window_msg_time_diagnostics); + EXPECT_FALSE(config.enable_fps_window_hdr_time_diagnostics); EXPECT_TRUE(config.enable_fps_window_node_time_diagnostics); - EXPECT_FALSE(config.enable_increasing_msg_time_diagnostics); + EXPECT_FALSE(config.enable_increasing_hdr_time_diagnostics); EXPECT_TRUE(config.enable_increasing_node_time_diagnostics); EXPECT_FALSE(config.fallback_to_nodetime); } -TEST_F(GreenwaveDiagnosticsTest, TimeCheckPresetHeaderWithFallbackNoMsgTimestamp) +TEST_F(GreenwaveDiagnosticsTest, TimeCheckPresetHeaderWithFallbackNoHdrTimestamp) { greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::HeaderWithFallback; - config.has_msg_timestamp = false; + config.has_hdr_timestamp = false; config.applyTimeCheckPreset(); EXPECT_TRUE(config.fallback_to_nodetime); EXPECT_TRUE(config.enable_node_time_diagnostics); EXPECT_FALSE(config.enable_fps_jitter_node_time_diagnostics); EXPECT_TRUE(config.enable_fps_window_node_time_diagnostics); EXPECT_TRUE(config.enable_increasing_node_time_diagnostics); - EXPECT_FALSE(config.enable_msg_time_diagnostics); - EXPECT_FALSE(config.enable_fps_jitter_msg_time_diagnostics); - EXPECT_FALSE(config.enable_fps_window_msg_time_diagnostics); - EXPECT_FALSE(config.enable_increasing_msg_time_diagnostics); + EXPECT_FALSE(config.enable_hdr_time_diagnostics); + EXPECT_FALSE(config.enable_fps_jitter_hdr_time_diagnostics); + EXPECT_FALSE(config.enable_fps_window_hdr_time_diagnostics); + EXPECT_FALSE(config.enable_increasing_hdr_time_diagnostics); } -TEST_F(GreenwaveDiagnosticsTest, TimeCheckPresetHeaderWithFallbackWithMsgTimestamp) +TEST_F(GreenwaveDiagnosticsTest, TimeCheckPresetHeaderWithFallbackWithHdrTimestamp) { greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.time_check_preset = greenwave_diagnostics::TimeCheckPreset::HeaderWithFallback; - config.has_msg_timestamp = true; + config.has_hdr_timestamp = true; config.applyTimeCheckPreset(); EXPECT_TRUE(config.fallback_to_nodetime); EXPECT_TRUE(config.enable_node_time_diagnostics); EXPECT_FALSE(config.enable_fps_jitter_node_time_diagnostics); EXPECT_FALSE(config.enable_fps_window_node_time_diagnostics); EXPECT_TRUE(config.enable_increasing_node_time_diagnostics); - EXPECT_TRUE(config.enable_msg_time_diagnostics); - EXPECT_TRUE(config.enable_fps_jitter_msg_time_diagnostics); - EXPECT_FALSE(config.enable_fps_window_msg_time_diagnostics); - EXPECT_TRUE(config.enable_increasing_msg_time_diagnostics); + EXPECT_TRUE(config.enable_hdr_time_diagnostics); + EXPECT_TRUE(config.enable_fps_jitter_hdr_time_diagnostics); + EXPECT_FALSE(config.enable_fps_window_hdr_time_diagnostics); + EXPECT_TRUE(config.enable_increasing_hdr_time_diagnostics); } diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index 5f93769..847afcb 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -226,7 +226,7 @@ def test_diagnostic_data_conversion(self, expected_frequency, message_type, tole from diagnostic_msgs.msg import KeyValue status.values = [ KeyValue(key='frame_rate_node', value='100.5'), - KeyValue(key='frame_rate_msg', value='99.8'), + KeyValue(key='frame_rate_hdr', value='99.8'), KeyValue(key='current_delay_from_realtime_ms', value='5.2') ]