Commit 8331314 1 parent 748e12b commit 8331314 Copy full SHA for 8331314
File tree 3 files changed +20
-0
lines changed
system/diagnostic_graph_aggregator/src/common/graph
3 files changed +20
-0
lines changed Original file line number Diff line number Diff line change @@ -210,6 +210,11 @@ DiagnosticGraph Graph::report(const rclcpp::Time & stamp)
210
210
}
211
211
message.nodes .push_back (temp);
212
212
}
213
+
214
+ for (const auto & [name, diag] : diags_) {
215
+ diag->reported ();
216
+ }
217
+
213
218
return message;
214
219
}
215
220
Original file line number Diff line number Diff line change @@ -64,6 +64,7 @@ BaseUnit::NodeData BaseUnit::report() const
64
64
65
65
void DiagUnit::init (const UnitConfig::SharedPtr & config, const NodeDict &)
66
66
{
67
+ is_reported_ = true ;
67
68
name_ = config->data .take_text (" diag" );
68
69
timeout_ = config->data .take <double >(" timeout" , 1.0 );
69
70
}
@@ -87,9 +88,21 @@ void DiagUnit::update(const rclcpp::Time & stamp)
87
88
88
89
void DiagUnit::callback (const rclcpp::Time & stamp, const DiagnosticStatus & status)
89
90
{
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 ;
90
98
diagnostics_ = std::make_pair (stamp, status);
91
99
}
92
100
101
+ void DiagUnit::reported ()
102
+ {
103
+ is_reported_ = true ;
104
+ }
105
+
93
106
AndUnit::AndUnit (const std::string & path, bool short_circuit) : BaseUnit(path)
94
107
{
95
108
short_circuit_ = short_circuit;
Original file line number Diff line number Diff line change @@ -82,8 +82,10 @@ class DiagUnit : public BaseUnit
82
82
83
83
std::string name () const { return name_; }
84
84
void callback (const rclcpp::Time & stamp, const DiagnosticStatus & status);
85
+ void reported ();
85
86
86
87
private:
88
+ bool is_reported_;
87
89
double timeout_;
88
90
std::optional<std::pair<rclcpp::Time, DiagnosticStatus>> diagnostics_;
89
91
std::string name_;
You can’t perform that action at this time.
0 commit comments