Re-use the publishData function such that also the actual diagnostics…

… are available

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()