diff --git a/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp b/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp index 5f48dd6b..176ae729 100644 --- a/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp +++ b/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp @@ -159,6 +159,11 @@ class Aggregator */ bool critical_; + /*! + *\brief If true, the aggregator will publish "values" for each DiagnosticStatus + */ + bool publish_values_; + /*! *\brief Store the last top level value to publish the critical error only once. */ diff --git a/diagnostic_aggregator/mainpage.dox b/diagnostic_aggregator/mainpage.dox index b720b86e..675823f2 100644 --- a/diagnostic_aggregator/mainpage.dox +++ b/diagnostic_aggregator/mainpage.dox @@ -50,6 +50,7 @@ Reads the following parameters from the parameter server - \b "~base_path" : \b double [optional] Prepended to all analyzed output - \b "~analyzers" : \b {} Configuration for loading analyzers - \b "~critical" : \b bool [optional] React immediately to a degradation in diagnostic state +- \b "~publish_values: \b bool [optional] Publish the "values" for each diagnostic status. Default: "true" \subsection analyzer_loader analyzer_loader diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index 4619ec29..aabd0b71 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/diagnostic_aggregator/src/aggregator.cpp @@ -67,6 +67,7 @@ Aggregator::Aggregator() clock_(n_->get_clock()), base_path_(""), critical_(false), + publish_values_(true), last_top_level_state_(DiagnosticStatus::STALE) { RCLCPP_DEBUG(logger_, "constructor"); @@ -123,6 +124,8 @@ void Aggregator::initAnalyzers() history_depth_ = param.second.as_int(); } else if (param.first.compare("critical") == 0) { critical_ = param.second.as_bool(); + } else if (param.first.compare("publish_values") == 0) { + publish_values_ = param.second.as_bool(); } } RCLCPP_DEBUG(logger_, "Aggregator publication rate configured to: %f", pub_rate_); @@ -131,6 +134,8 @@ void Aggregator::initAnalyzers() logger_, "Aggregator other_as_errors configured to: %s", (other_as_errors ? "true" : "false")); RCLCPP_DEBUG( logger_, "Aggregator critical publisher configured to: %s", (critical_ ? "true" : "false")); + RCLCPP_DEBUG( + logger_, "Aggregator publish_values configured to: %s", (publish_values_ ? "true" : "false")); { // lock the mutex while analyzer_group_ and other_analyzer_ are being updated std::lock_guard lock(mutex_); @@ -244,6 +249,13 @@ void Aggregator::publishData() } } + // If "publish_values" is false, clear all values + if (!publish_values_) { + for (auto & status : diag_array.status) { + status.values.clear(); + } + } + diag_array.header.stamp = clock_->now(); agg_pub_->publish(diag_array);