37 #ifndef __DIAGNOSTIC_STATUS__UPDATE_FUNCTIONS_H__ 38 #define __DIAGNOSTIC_STATUS__UPDATE_FUNCTIONS_H__ 144 boost::mutex::scoped_lock lock(lock_);
151 seq_nums_[i] = count_;
162 boost::mutex::scoped_lock lock(lock_);
169 boost::mutex::scoped_lock lock(lock_);
172 int events = curseq - seq_nums_[hist_indx_];
173 double window = (curtime - times_[hist_indx_]).toSec();
174 double freq = events / window;
175 seq_nums_[hist_indx_] = curseq;
176 times_[hist_indx_] = curtime;
181 stat.
summary(2,
"No events recorded.");
185 stat.
summary(1,
"Frequency too low.");
189 stat.
summary(1,
"Frequency too high.");
193 stat.
summary(0,
"Desired frequency met");
196 stat.
addf(
"Events in window",
"%d", events);
197 stat.
addf(
"Events since startup",
"%d", count_);
198 stat.
addf(
"Duration of window (s)",
"%f", window);
199 stat.
addf(
"Actual frequency (Hz)",
"%f",freq);
203 stat.
addf(
"Minimum acceptable frequency (Hz)",
"%f",
211 stat.
addf(
"Maximum acceptable frequency (Hz)",
"%f",
228 max_acceptable_(max_acceptable), min_acceptable_(min_acceptable)
274 deltas_valid_ =
false;
321 boost::mutex::scoped_lock lock(lock_);
331 if (!deltas_valid_ || delta > max_delta_)
334 if (!deltas_valid_ || delta < min_delta_)
337 deltas_valid_ =
true;
355 boost::mutex::scoped_lock lock(lock_);
357 stat.
summary(0,
"Timestamps are reasonable.");
360 stat.
summary(1,
"No data since last update.");
364 if (min_delta_ < params_.min_acceptable_)
366 stat.
summary(2,
"Timestamps too far in future seen.");
370 if (max_delta_ > params_.max_acceptable_)
372 stat.
summary(2,
"Timestamps too far in past seen.");
378 stat.
summary(2,
"Zero timestamp seen.");
383 stat.
addf(
"Earliest timestamp delay:",
"%f", min_delta_);
384 stat.
addf(
"Latest timestamp delay:",
"%f", max_delta_);
385 stat.
addf(
"Earliest acceptable timestamp delay:",
"%f", params_.min_acceptable_);
386 stat.
addf(
"Latest acceptable timestamp delay:",
"%f", params_.max_acceptable_);
387 stat.
add(
"Late diagnostic update count:", late_count_);
388 stat.
add(
"Early diagnostic update count:", early_count_);
389 stat.
add(
"Zero seen diagnostic update count:", zero_count_);
391 deltas_valid_ =
false;
static TimeStampStatusParam DefaultTimeStampStatusParam
Default TimeStampStatusParam. This is like calling the constructor with no arguments.
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Fills out this Task's DiagnosticStatusWrapper.
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Fills out this Task's DiagnosticStatusWrapper.
double * min_freq_
Minimum acceptable frequency.
TimeStampStatus(const TimeStampStatusParam ¶ms, std::string name)
Constructs the TimeStampStatus with the given parameters.
void summary(unsigned char lvl, const std::string s)
Fills out the level and message fields of the DiagnosticStatus.
TimeStampStatus()
Constructs the TimeStampStatus with the default parameters. Uses a default diagnostic task name of "T...
A structure that holds the constructor parameters for the FrequencyStatus class.
void init(const M_string &remappings)
int window_size_
Number of events to consider in the statistics.
std::vector< ros::Time > times_
A structure that holds the constructor parameters for the TimeStampStatus class.
FrequencyStatus(const FrequencyStatusParam ¶ms, std::string name)
Constructs a FrequencyStatus class with the given parameters.
void addf(const std::string &key, const char *format,...)
Add a key-value pair using a format string.
double max_acceptable_
Maximum acceptable difference between two timestamps.
A diagnostic task that monitors the frequency of an event.
double tolerance_
Tolerance with which bounds must be satisfied.
Heartbeat()
Constructs a HeartBeat.
const FrequencyStatusParam params_
void tick()
Signals that an event has occurred.
double min_acceptable_
Minimum acceptable difference between two timestamps.
TimeStampStatus(const TimeStampStatusParam ¶ms)
Constructs the TimeStampStatus with the given parameters. Uses a default diagnostic task name of "Tim...
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Fills out this Task's DiagnosticStatusWrapper.
Diagnostic task to monitor the interval between events.
FrequencyStatusParam(double *min_freq, double *max_freq, double tolerance=0.1, int window_size=5)
Creates a filled-out FrequencyStatusParam.
void clear()
Resets the statistics.
TimeStampStatusParam(const double min_acceptable=-1, const double max_acceptable=5)
Creates a filled-out TimeStampStatusParam.
FrequencyStatus(const FrequencyStatusParam ¶ms)
Constructs a FrequencyStatus class with the given parameters. Uses a default diagnostic task name of ...
TimeStampStatusParam params_
double * max_freq_
Maximum acceptable frequency.
DiagnosticTask is an abstract base class for collecting diagnostic data.
Diagnostic task to monitor whether a node is alive.
void add(const std::string &key, const T &val)
Add a key-value pair.
void tick(double stamp)
Signals an event. Timestamp stored as a double.
void tick(const ros::Time t)
Signals an event.
std::vector< int > seq_nums_
Wrapper for the diagnostic_msgs::DiagnosticStatus message that makes it easier to update...