diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index bf867a41..cbddb04c 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/diagnostic_aggregator/src/aggregator.cpp @@ -221,6 +221,8 @@ void Aggregator::publishData() diag_toplevel_state.level = DiagnosticStatus::STALE; int max_level = -1; int min_level = 255; + uint non_ok_status_depth = 0; + std::shared_ptr msg_to_report; std::vector> processed; { @@ -229,27 +231,61 @@ void Aggregator::publishData() } for (const auto & msg : processed) { diag_array.status.push_back(*msg); + const uint depth = static_cast(std::count(msg->name.begin(), msg->name.end(), '/')); if (msg->level > max_level) { max_level = msg->level; + non_ok_status_depth = depth; + msg_to_report = msg; + } + if (msg->level == max_level && depth > non_ok_status_depth) + { + // On non okay diagnostics also copy the deepest message to toplevel state + non_ok_status_depth = depth; + msg_to_report = msg; } if (msg->level < min_level) { min_level = msg->level; } } + // When a non-ok item was found, copy the complete status message once + if (max_level > DiagnosticStatus::OK) + { + diag_toplevel_state.name = msg_to_report->name; + diag_toplevel_state.message = msg_to_report->message; + diag_toplevel_state.hardware_id = msg_to_report->hardware_id; + diag_toplevel_state.values = msg_to_report->values; + } std::vector> processed_other = other_analyzer_->report(); for (const auto & msg : processed_other) { diag_array.status.push_back(*msg); + const uint depth = static_cast(std::count(msg->name.begin(), msg->name.end(), '/')); if (msg->level > max_level) { max_level = msg->level; + non_ok_status_depth = depth; + msg_to_report = msg; + } + if (msg->level == max_level && depth > non_ok_status_depth) + { + // On non okay diagnostics also copy the deepest message to toplevel state + non_ok_status_depth = depth; + msg_to_report = msg; } if (msg->level < min_level) { min_level = msg->level; } } + // When a non-ok item was found, copy the complete status message once + if (max_level > DiagnosticStatus::OK) + { + diag_toplevel_state.name = msg_to_report->name; + diag_toplevel_state.message = msg_to_report->message; + diag_toplevel_state.hardware_id = msg_to_report->hardware_id; + diag_toplevel_state.values = msg_to_report->values; + } diag_array.header.stamp = clock_->now(); agg_pub_->publish(diag_array);