From ef48cc6b64fcd4d75ddf8ef773ae067db013f282 Mon Sep 17 00:00:00 2001
From: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Date: Mon, 17 Apr 2023 16:44:50 +0200
Subject: [PATCH] Report STALE when there is at least one STALE and no ERROR

---
 diagnostic_aggregator/src/aggregator.cpp     | 27 ++++++++++++--------
 diagnostic_aggregator/src/analyzer_group.cpp | 19 +++++++++-----
 2 files changed, 30 insertions(+), 16 deletions(-)

diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp
index 1000dd422..83f037d80 100644
--- a/diagnostic_aggregator/src/aggregator.cpp
+++ b/diagnostic_aggregator/src/aggregator.cpp
@@ -226,7 +226,7 @@ void Aggregator::publishData()
   diagnostic_msgs::DiagnosticStatus diag_toplevel_state;
   diag_toplevel_state.name = "toplevel_state";
   diag_toplevel_state.level = -1;
-  int min_level = 255;
+  int8_t max_level_without_stale = 0;
   uint non_ok_status_depth = 0;
   uint report_idx = 0;
 
@@ -252,9 +252,10 @@ void Aggregator::publishData()
       non_ok_status_depth = depth;
       report_idx = i;
     }
-    if (processed[i]->level < min_level)
-    {
-      min_level = processed[i]->level;
+    if (
+      processed[i]->level > max_level_without_stale &&
+      processed[i]->level != diagnostic_msgs::DiagnosticStatus::STALE) {
+      max_level_without_stale = processed[i]->level;
     }
   }
   // When a non-ok item was found, copy the complete status message once
@@ -292,9 +293,10 @@ void Aggregator::publishData()
       non_ok_status_depth = depth;
       report_idx = i;
     }
-    if (processed_other[i]->level < min_level)
-    {
-      min_level = processed_other[i]->level;
+    if (
+      processed_other[i]->level > max_level_without_stale &&
+      processed_other[i]->level != diagnostic_msgs::DiagnosticStatus::STALE) {
+      max_level_without_stale = processed_other[i]->level;
     }
     // When a non-ok item was found AND it was reported in 'other' categpry, copy the complete status message once
     if (diag_toplevel_state.level > diagnostic_msgs::DiagnosticStatus::OK && non_ok_status_depth > 0)
@@ -310,9 +312,14 @@ void Aggregator::publishData()
 
   agg_pub_.publish(diag_array);
 
-  // Top level is error if we have stale items, unless all stale
-  if (diag_toplevel_state.level > int(DiagnosticLevel::Level_Error) && min_level <= int(DiagnosticLevel::Level_Error))
-    diag_toplevel_state.level = DiagnosticLevel::Level_Error;
+  // If there is at least one stale message and there are no errors, report stale
+  if (
+    diag_toplevel_state.level == DiagnosticLevel::Level_Stale &&
+    max_level_without_stale < DiagnosticLevel::Level_Error) {
+    diag_toplevel_state.level = DiagnosticLevel::Level_Stale;
+  } else {
+    diag_toplevel_state.level = max_level_without_stale;
+  }
 
   // Store latest toplevel state for immediate publish checking
   diag_toplevel_ = diag_toplevel_state.level;
diff --git a/diagnostic_aggregator/src/analyzer_group.cpp b/diagnostic_aggregator/src/analyzer_group.cpp
index 88cac8a5f..77579109d 100644
--- a/diagnostic_aggregator/src/analyzer_group.cpp
+++ b/diagnostic_aggregator/src/analyzer_group.cpp
@@ -253,7 +253,7 @@ vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > AnalyzerGroup::rep
     return output;
   }
 
-  bool all_stale = true;
+  int8_t max_level_without_stale = 0;
 
   for (unsigned int j = 0; j < analyzers_.size(); ++j)
   {
@@ -278,17 +278,24 @@ vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > AnalyzerGroup::rep
         diagnostic_msgs::KeyValue kv;
         kv.key = nice_name;
         kv.value = processed[i]->message;
-        
-        all_stale = all_stale && (processed[i]->level == int(DiagnosticLevel::Level_Stale));
+
+        if (processed[i]->level != DiagnosticLevel::Level_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 == int(DiagnosticLevel::Level_Stale) && !all_stale)
-    header_status->level = DiagnosticLevel::Level_Error;
+  // If one STALE and no ERROR, report STALE
+  if (
+    header_status->level == DiagnosticLevel::Level_Stale &&
+    max_level_without_stale < DiagnosticLevel::Level_Error) {
+    header_status->level = DiagnosticLevel::Level_Stale;
+  } else {
+    header_status->level = max_level_without_stale;
+  }
 
   header_status->message = valToMsg(header_status->level);