Skip to content

Commit 8331314

Browse files
authored
feat(diagnostic_graph_aggregator): report the highest error level (#1248)
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 748e12b commit 8331314

File tree

3 files changed

+20
-0
lines changed

3 files changed

+20
-0
lines changed

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

+5
Original file line numberDiff line numberDiff line change
@@ -210,6 +210,11 @@ DiagnosticGraph Graph::report(const rclcpp::Time & stamp)
210210
}
211211
message.nodes.push_back(temp);
212212
}
213+
214+
for (const auto & [name, diag] : diags_) {
215+
diag->reported();
216+
}
217+
213218
return message;
214219
}
215220

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

+13
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,7 @@ BaseUnit::NodeData BaseUnit::report() const
6464

6565
void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &)
6666
{
67+
is_reported_ = true;
6768
name_ = config->data.take_text("diag");
6869
timeout_ = config->data.take<double>("timeout", 1.0);
6970
}
@@ -87,9 +88,21 @@ void DiagUnit::update(const rclcpp::Time & stamp)
8788

8889
void DiagUnit::callback(const rclcpp::Time & stamp, const DiagnosticStatus & status)
8990
{
91+
// Do not overwrite if high error level has not been reported yet.
92+
if (diagnostics_ && !is_reported_) {
93+
if (status.level < diagnostics_->second.level) {
94+
return;
95+
}
96+
}
97+
is_reported_ = false;
9098
diagnostics_ = std::make_pair(stamp, status);
9199
}
92100

101+
void DiagUnit::reported()
102+
{
103+
is_reported_ = true;
104+
}
105+
93106
AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path)
94107
{
95108
short_circuit_ = short_circuit;

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

+2
Original file line numberDiff line numberDiff line change
@@ -82,8 +82,10 @@ class DiagUnit : public BaseUnit
8282

8383
std::string name() const { return name_; }
8484
void callback(const rclcpp::Time & stamp, const DiagnosticStatus & status);
85+
void reported();
8586

8687
private:
88+
bool is_reported_;
8789
double timeout_;
8890
std::optional<std::pair<rclcpp::Time, DiagnosticStatus>> diagnostics_;
8991
std::string name_;

0 commit comments

Comments
 (0)