Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions docs/DESIGN_AND_IMPLEMENTATION.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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`
Expand All @@ -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. |

Expand All @@ -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.
Expand Down
4 changes: 2 additions & 2 deletions greenwave_monitor/greenwave_monitor/test_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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")

Expand Down
2 changes: 1 addition & 1 deletion greenwave_monitor/greenwave_monitor/ui_adaptor.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
130 changes: 65 additions & 65 deletions greenwave_monitor/include/greenwave_diagnostics.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand All @@ -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;
}
}
Expand Down Expand Up @@ -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;
Expand All @@ -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<std::mutex> 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<uint64_t>(
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<uint64_t>(
clock_->now().nanoseconds() / constants::kMicrosecondsToNanoseconds);
message_latency_msg_ms_ =
static_cast<double>(ros_node_system_time_us - current_timestamp_msg_us) /
message_latency_hdr_ms_ =
static_cast<double>(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<double>::quiet_NaN();
if (message_latency_hdr_ms_ > constants::kNonsenseLatencyMs) {
message_latency_hdr_ms_ = std::numeric_limits<double>::quiet_NaN();
}
}

Expand All @@ -239,50 +239,50 @@ class GreenwaveDiagnostics
} else {
values.push_back(
diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()
.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<diagnostic_msgs::msg::KeyValue>()
.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<diagnostic_msgs::msg::KeyValue>()
.key("num_jitter_outliers_node")
.value(std::to_string(node_ts_series_.window.outlier_count)));
values.push_back(
diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()
.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<diagnostic_msgs::msg::KeyValue>()
.key("max_abs_jitter_node")
.value(std::to_string(node_ts_series_.window.max_abs_jitter_us)));
values.push_back(
diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()
.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<diagnostic_msgs::msg::KeyValue>()
.key("mean_abs_jitter_node")
.value(std::to_string(node_ts_series_.window.meanAbsJitterUs())));
values.push_back(
diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()
.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<diagnostic_msgs::msg::KeyValue>()
.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<diagnostic_msgs::msg::KeyValue>()
.key("frame_rate_node")
.value(std::to_string(node_ts_series_.window.frameRateHz())));
values.push_back(
diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()
.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<diagnostic_msgs::msg::KeyValue>()
.key("expected_frequency")
Expand Down Expand Up @@ -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<std::mutex> 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) {
Expand Down Expand Up @@ -363,7 +363,7 @@ class GreenwaveDiagnostics
{
const std::lock_guard<std::mutex> 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;
Expand Down Expand Up @@ -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<double>::quiet_NaN()};
double message_latency_hdr_ms_{std::numeric_limits<double>::quiet_NaN()};
bool outdated_msg_{true};
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_publisher_;

Expand Down
6 changes: 3 additions & 3 deletions greenwave_monitor/src/greenwave_monitor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -148,8 +148,8 @@ void GreenwaveMonitor::topic_callback(
const std::shared_ptr<rclcpp::SerializedMessage> 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()
Expand Down Expand Up @@ -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(
Expand Down
4 changes: 2 additions & 2 deletions greenwave_monitor/src/minimal_publisher_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Loading
Loading