From 834e25ef5ac9a35ba02cd5c8ff68fa33da6a59fe Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 29 Apr 2024 13:51:41 +0200 Subject: [PATCH 1/4] ROS2 port of feature to report stale when no errors but stale items --- diagnostic_aggregator/src/aggregator.cpp | 30 ++++++++++++-------- diagnostic_aggregator/src/analyzer_group.cpp | 17 +++++++---- 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index 2eb94f3c6..8941ab53e 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/diagnostic_aggregator/src/aggregator.cpp @@ -221,7 +221,7 @@ void Aggregator::publishData() diag_toplevel_state.name = "toplevel_state"; diag_toplevel_state.level = DiagnosticStatus::STALE; int max_level = -1; - int min_level = 255; + int8_t max_level_without_stale = 0; int non_ok_status_depth = 0; std::shared_ptr msg_to_report; @@ -244,8 +244,10 @@ void Aggregator::publishData() non_ok_status_depth = depth; msg_to_report = msg; } - if (msg->level < min_level) { - min_level = msg->level; + if ( + msg->level > max_level_without_stale && + msg->level != diagnostic_msgs::msg::DiagnosticStatus::STALE) { + max_level_without_stale = msg->level; } } // When a non-ok item was found, copy the complete status message once @@ -272,8 +274,10 @@ void Aggregator::publishData() non_ok_status_depth = depth; msg_to_report = msg; } - if (msg->level < min_level) { - min_level = msg->level; + if ( + msg->level > max_level_without_stale && + msg->level != diagnostic_msgs::msg::DiagnosticStatus::STALE) { + max_level_without_stale = msg->level; } } // When a non-ok item was found, copy the complete status message once @@ -287,14 +291,16 @@ void Aggregator::publishData() diag_array.header.stamp = clock_->now(); agg_pub_->publish(diag_array); - diag_toplevel_state.level = max_level; - if (max_level < 0 || - (max_level > DiagnosticStatus::ERROR && min_level <= DiagnosticStatus::ERROR)) - { - // Top level is error if we got no diagnostic level or - // have stale items but not all are stale - diag_toplevel_state.level = DiagnosticStatus::ERROR; + if ( + diag_toplevel_state.level == diagnostic_msgs::msg::DiagnosticStatus::STALE && + max_level_without_stale < diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + diag_toplevel_state.level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + } else { + diag_toplevel_state.level = max_level_without_stale; } + + + last_top_level_state_ = diag_toplevel_state.level; toplevel_state_pub_->publish(diag_toplevel_state); diff --git a/diagnostic_aggregator/src/analyzer_group.cpp b/diagnostic_aggregator/src/analyzer_group.cpp index 0873ac4c5..58ebb2598 100644 --- a/diagnostic_aggregator/src/analyzer_group.cpp +++ b/diagnostic_aggregator/src/analyzer_group.cpp @@ -292,7 +292,7 @@ std::vector> AnalyzerGro return output; } - bool all_stale = true; + unsigned char max_level_without_stale = 0; for (auto j = 0u; j < analyzers_.size(); ++j) { std::string path = analyzers_[j]->getPath(); @@ -317,17 +317,22 @@ std::vector> AnalyzerGro kv.key = nice_name; kv.value = processed[i]->message; - all_stale = all_stale && - (processed[i]->level == diagnostic_msgs::msg::DiagnosticStatus::STALE); + if (processed[i]->level != diagnostic_msgs::msg::DiagnosticStatus::STALE) { + max_level_without_stale = max(max_level_without_stale, processed[i]->level); + } header_status->level = max(header_status->level, processed[i]->level); header_status->values.push_back(kv); } } } - // Report stale as errors unless all stale - if (header_status->level == diagnostic_msgs::msg::DiagnosticStatus::STALE && !all_stale) { - header_status->level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + // If one STALE and no ERROR, report STALE + if ( + header_status->level == diagnostic_msgs::msg::DiagnosticStatus::STALE && + max_level_without_stale < diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + header_status->level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + } else { + header_status->level = max_level_without_stale; } header_status->message = valToMsg(header_status->level); From f2c31eb836129a421f6bfb8b3f1562c089a2a21b Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 29 Apr 2024 15:14:07 +0200 Subject: [PATCH 2/4] Fixup! --- diagnostic_aggregator/src/aggregator.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index 8941ab53e..ebc953bf6 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/diagnostic_aggregator/src/aggregator.cpp @@ -258,6 +258,7 @@ void Aggregator::publishData() diag_toplevel_state.values = msg_to_report->values; } + non_ok_status_depth = 0; std::vector> processed_other = other_analyzer_->report(); for (const auto & msg : processed_other) { @@ -292,7 +293,7 @@ void Aggregator::publishData() agg_pub_->publish(diag_array); if ( - diag_toplevel_state.level == diagnostic_msgs::msg::DiagnosticStatus::STALE && + max_level == diagnostic_msgs::msg::DiagnosticStatus::STALE && max_level_without_stale < diagnostic_msgs::msg::DiagnosticStatus::ERROR) { diag_toplevel_state.level = diagnostic_msgs::msg::DiagnosticStatus::STALE; } else { From ecdc89e7ba797a4afef88dd8934a23de763adbe7 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 29 Apr 2024 15:26:18 +0200 Subject: [PATCH 3/4] uncrustify --- diagnostic_aggregator/src/aggregator.cpp | 10 ++++++---- diagnostic_aggregator/src/analyzer_group.cpp | 3 ++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index ebc953bf6..9fbb3cb5f 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/diagnostic_aggregator/src/aggregator.cpp @@ -246,7 +246,8 @@ void Aggregator::publishData() } if ( msg->level > max_level_without_stale && - msg->level != diagnostic_msgs::msg::DiagnosticStatus::STALE) { + msg->level != diagnostic_msgs::msg::DiagnosticStatus::STALE) + { max_level_without_stale = msg->level; } } @@ -277,7 +278,8 @@ void Aggregator::publishData() } if ( msg->level > max_level_without_stale && - msg->level != diagnostic_msgs::msg::DiagnosticStatus::STALE) { + msg->level != diagnostic_msgs::msg::DiagnosticStatus::STALE) + { max_level_without_stale = msg->level; } } @@ -294,14 +296,14 @@ void Aggregator::publishData() if ( max_level == diagnostic_msgs::msg::DiagnosticStatus::STALE && - max_level_without_stale < diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + max_level_without_stale < diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { diag_toplevel_state.level = diagnostic_msgs::msg::DiagnosticStatus::STALE; } else { diag_toplevel_state.level = max_level_without_stale; } - last_top_level_state_ = diag_toplevel_state.level; toplevel_state_pub_->publish(diag_toplevel_state); diff --git a/diagnostic_aggregator/src/analyzer_group.cpp b/diagnostic_aggregator/src/analyzer_group.cpp index 58ebb2598..d54a70d39 100644 --- a/diagnostic_aggregator/src/analyzer_group.cpp +++ b/diagnostic_aggregator/src/analyzer_group.cpp @@ -329,7 +329,8 @@ std::vector> AnalyzerGro // If one STALE and no ERROR, report STALE if ( header_status->level == diagnostic_msgs::msg::DiagnosticStatus::STALE && - max_level_without_stale < diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + max_level_without_stale < diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { header_status->level = diagnostic_msgs::msg::DiagnosticStatus::STALE; } else { header_status->level = max_level_without_stale; From 5ebceeb669a9989f20102cf0e3cba15b96472ead Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 30 Apr 2024 15:00:51 +0200 Subject: [PATCH 4/4] Update test since we change this outcome --- diagnostic_aggregator/test/test_discard_behavior.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/diagnostic_aggregator/test/test_discard_behavior.py b/diagnostic_aggregator/test/test_discard_behavior.py index 16dee0162..ddc19dd5f 100644 --- a/diagnostic_aggregator/test/test_discard_behavior.py +++ b/diagnostic_aggregator/test/test_discard_behavior.py @@ -117,7 +117,7 @@ foo_status=None, bar_discard=False, bar_status=None, - agg_expected=DiagnosticStatus.ERROR, # <-- This is the case we are testing for. + agg_expected=DiagnosticStatus.STALE, # <-- This is the case we are testing for. # if one of the children is *not* marked discard_stale := true and # there are no statuses, then the parent should roll up to ERROR. ), @@ -126,7 +126,7 @@ foo_status=DiagnosticStatus.OK, bar_discard=False, bar_status=None, - agg_expected=DiagnosticStatus.ERROR, + agg_expected=DiagnosticStatus.STALE, ), TestMetadata( foo_discard=True,