From 83313143b900f4509b2fe0202de3abefae14dc02 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com>
Date: Fri, 12 Apr 2024 10:27:34 +0900
Subject: [PATCH] feat(diagnostic_graph_aggregator): report the highest error
 level (#1248)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../src/common/graph/graph.cpp                      |  5 +++++
 .../src/common/graph/units.cpp                      | 13 +++++++++++++
 .../src/common/graph/units.hpp                      |  2 ++
 3 files changed, 20 insertions(+)

diff --git a/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp b/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp
index 5d7c11879a9cc..b31e1a23fb395 100644
--- a/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp
+++ b/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;
 }
 
diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp
index 6048cae85e633..99ef020de271e 100644
--- a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp
+++ b/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;
diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.hpp b/system/diagnostic_graph_aggregator/src/common/graph/units.hpp
index dce223b30f728..f20ea528f6ca3 100644
--- a/system/diagnostic_graph_aggregator/src/common/graph/units.hpp
+++ b/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_;