Sync nobleo ros2 june 2026

diagnostic_aggregator/include/diagnostic_aggregator/status_item.hpp

@@ -98,7 +98,7 @@ inline DiagnosticLevel valToLevel(const int val)
     return Level_Stale;
   }
 
-  RCLCPP_ERROR(
+  RCLCPP_DEBUG(
     rclcpp::get_logger(
       "generic_analyzer_base"),
     R"(Attempting to convert %d into DiagnosticLevel.

diagnostic_aggregator/src/aggregator.cpp

@@ -265,7 +265,15 @@ void Aggregator::publishData()
       max_level_without_stale = msg->level;
     }
   }
+  // When a non-ok item was found, copy the complete status message once
+  if (max_level > DiagnosticStatus::OK) {
+    diag_toplevel_state.name = msg_to_report->name;
+    diag_toplevel_state.message = msg_to_report->message;
+    diag_toplevel_state.hardware_id = msg_to_report->hardware_id;
+    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) {
@@ -290,14 +298,6 @@ void Aggregator::publishData()
     }
   }
 
-  // When a non-ok item was found, surface the offender via message/hardware_id/values
-  // but keep name stable as "toplevel_state" to avoid breaking downstream consumers
-  if (max_level > DiagnosticStatus::OK && msg_to_report) {
-    diag_toplevel_state.message = msg_to_report->name + ": " + msg_to_report->message;
-    diag_toplevel_state.hardware_id = msg_to_report->hardware_id;
-    diag_toplevel_state.values = msg_to_report->values;
-  }
-
   // If "publish_values" is false, clear all values
   if (!publish_values_) {
     for (auto & status : diag_array.status) {
@@ -308,14 +308,13 @@ void Aggregator::publishData()
   diag_array.header.stamp = clock_->now();
   agg_pub_->publish(diag_array);
 
-  if (max_level_without_stale > DiagnosticStatus::OK) {
-    diag_toplevel_state.level = max_level_without_stale;
-  } else if (max_level == diagnostic_msgs::msg::DiagnosticStatus::STALE) {
-    diag_toplevel_state.level = diagnostic_msgs::msg::DiagnosticStatus::STALE;
-  } else if (max_level < 0) {
+  if (
+    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 {
-    diag_toplevel_state.level = DiagnosticStatus::OK;
+    diag_toplevel_state.level = max_level_without_stale;
   }
 
   last_top_level_state_ = diag_toplevel_state.level;

diagnostic_aggregator/src/analyzer_group.cpp

@@ -292,7 +292,7 @@ std::vector<std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus>> AnalyzerGro
     return output;
   }
 
-  uint8_t max_level_without_stale = 0;
+  unsigned char max_level_without_stale = 0;
 
   for (auto j = 0u; j < analyzers_.size(); ++j) {
     std::string path = analyzers_[j]->getPath();
@@ -326,13 +326,14 @@ std::vector<std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus>> AnalyzerGro
     }
   }
 
-  // WARN/ERROR always beats STALE; if only STALE present, report STALE
-  if (max_level_without_stale > diagnostic_msgs::msg::DiagnosticStatus::OK) {
-    header_status->level = max_level_without_stale;
-  } else if (header_status->level == diagnostic_msgs::msg::DiagnosticStatus::STALE) {
+  // 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 = diagnostic_msgs::msg::DiagnosticStatus::OK;
+    header_status->level = max_level_without_stale;
   }
 
   header_status->message = valToMsg(header_status->level);