ROS2 port of feature to report stale when no errors but stale items

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<DiagnosticStatus> msg_to_report;
 
@@ -244,8 +244,11 @@ 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
@@ -256,6 +259,7 @@ void Aggregator::publishData()
     diag_toplevel_state.values = msg_to_report->values;
   }
 
+  non_ok_status_depth = 0;
   std::vector<std::shared_ptr<DiagnosticStatus>> processed_other =
     other_analyzer_->report();
   for (const auto & msg : processed_other) {
@@ -272,8 +276,11 @@ 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 +294,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))
+  if (
+    max_level == diagnostic_msgs::msg::DiagnosticStatus::STALE &&
+    max_level_without_stale < diagnostic_msgs::msg::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;
+    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);

diagnostic_aggregator/src/analyzer_group.cpp

@@ -292,7 +292,7 @@ std::vector<std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus>> 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,23 @@ std::vector<std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus>> 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);

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,