feat(diagnostic_graph_aggregator): report the highest error level (#1248)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Author: isamu-takagi

system/diagnostic_graph_aggregator/src/common/graph/graph.cpp

@@ -210,6 +210,11 @@ DiagnosticGraph Graph::report(const rclcpp::Time & stamp)
     }
     message.nodes.push_back(temp);
   }
+
+  for (const auto & [name, diag] : diags_) {
+    diag->reported();
+  }
+
   return message;
 }
 

system/diagnostic_graph_aggregator/src/common/graph/units.cpp

@@ -64,6 +64,7 @@ BaseUnit::NodeData BaseUnit::report() const
 
 void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &)
 {
+  is_reported_ = true;
   name_ = config->data.take_text("diag");
   timeout_ = config->data.take<double>("timeout", 1.0);
 }
@@ -87,9 +88,21 @@ void DiagUnit::update(const rclcpp::Time & stamp)
 
 void DiagUnit::callback(const rclcpp::Time & stamp, const DiagnosticStatus & status)
 {
+  // Do not overwrite if high error level has not been reported yet.
+  if (diagnostics_ && !is_reported_) {
+    if (status.level < diagnostics_->second.level) {
+      return;
+    }
+  }
+  is_reported_ = false;
   diagnostics_ = std::make_pair(stamp, status);
 }
 
+void DiagUnit::reported()
+{
+  is_reported_ = true;
+}
+
 AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path)
 {
   short_circuit_ = short_circuit;

system/diagnostic_graph_aggregator/src/common/graph/units.hpp

@@ -82,8 +82,10 @@ class DiagUnit : public BaseUnit
 
   std::string name() const { return name_; }
   void callback(const rclcpp::Time & stamp, const DiagnosticStatus & status);
+  void reported();
 
 private:
+  bool is_reported_;
   double timeout_;
   std::optional<std::pair<rclcpp::Time, DiagnosticStatus>> diagnostics_;
   std::string name_;