From 3fbc372013bdddfd6059c92df85b2f1f310fb24c Mon Sep 17 00:00:00 2001
From: Tim Clephas <tim.clephas@nobleo.nl>
Date: Thu, 21 Dec 2023 14:33:59 +0100
Subject: [PATCH] Re-use the publishData function such that also the actual
 diagnostics are available

---
 diagnostic_aggregator/src/aggregator.cpp | 27 +++++++++---------------
 1 file changed, 10 insertions(+), 17 deletions(-)

diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp
index e17ef036..64b716ca 100644
--- a/diagnostic_aggregator/src/aggregator.cpp
+++ b/diagnostic_aggregator/src/aggregator.cpp
@@ -151,27 +151,11 @@ void Aggregator::diagCallback(const DiagnosticArray::SharedPtr diag_msg)
   checkTimestamp(diag_msg);
 
   bool analyzed = false;
+  bool immediate_report = false;
   {  // lock the whole loop to ensure nothing in the analyzer group changes during it.
     std::lock_guard<std::mutex> lock(mutex_);
     for (auto j = 0u; j < diag_msg->status.size(); ++j) {
       analyzed = false;
-
-      const bool top_level_state_degradation = (last_top_level_state_ < diag_msg->status[j].level);
-
-      if (critical_ && top_level_state_degradation) {
-        RCLCPP_DEBUG(
-          logger_, "Received degrated message: %s, publishing immediately",
-          diag_msg->status[j].name.c_str());
-
-        DiagnosticStatus diag_toplevel_state;
-        diag_toplevel_state.name = "toplevel_state_critical";
-        diag_toplevel_state.level = diag_msg->status[j].level;
-        toplevel_state_pub_->publish(diag_toplevel_state);
-
-        // store the last published state
-        last_top_level_state_ = diag_toplevel_state.level;
-      }
-
       auto item = std::make_shared<StatusItem>(&diag_msg->status[j]);
 
       if (analyzer_group_->match(item->getName())) {
@@ -181,8 +165,17 @@ void Aggregator::diagCallback(const DiagnosticArray::SharedPtr diag_msg)
       if (!analyzed) {
         other_analyzer_->analyze(item);
       }
+
+      // In case there is a degraded state, publish immediately
+      if (critical_ && item->getLevel() > last_top_level_state_) {
+        immediate_report = true;
+      }
     }
   }
+
+  if (immediate_report) {
+    publishData();
+  }
 }
 
 Aggregator::~Aggregator()