00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00036
00037 #ifndef __DIAGNOSTIC_STATUS__UPDATE_FUNCTIONS_H__
00038 #define __DIAGNOSTIC_STATUS__UPDATE_FUNCTIONS_H__
00039
00040 #include <diagnostic_updater/diagnostic_updater.h>
00041 #include <math.h>
00042
00043 namespace diagnostic_updater
00044 {
00045
00050 struct FrequencyStatusParam
00051 {
00056 FrequencyStatusParam(double *min_freq, double *max_freq, double tolerance = 0.1, int window_size = 5) :
00057 min_freq_(min_freq), max_freq_(max_freq), tolerance_(tolerance), window_size_(window_size)
00058 {}
00059
00066 double *min_freq_;
00067
00074 double *max_freq_;
00075
00086 double tolerance_;
00087
00091 int window_size_;
00092 };
00093
00103 class FrequencyStatus : public DiagnosticTask
00104 {
00105 private:
00106 const FrequencyStatusParam params_;
00107
00108 int count_;
00109 std::vector <ros::Time> times_;
00110 std::vector <int> seq_nums_;
00111 int hist_indx_;
00112 boost::mutex lock_;
00113
00114 public:
00119 FrequencyStatus(const FrequencyStatusParam ¶ms, std::string name) :
00120 DiagnosticTask(name), params_(params),
00121 times_(params_.window_size_), seq_nums_(params_.window_size_)
00122 {
00123 clear();
00124 }
00125
00131 FrequencyStatus(const FrequencyStatusParam ¶ms) :
00132 DiagnosticTask("Frequency Status"), params_(params),
00133 times_(params_.window_size_), seq_nums_(params_.window_size_)
00134 {
00135 clear();
00136 }
00137
00142 void clear()
00143 {
00144 boost::mutex::scoped_lock lock(lock_);
00145 ros::Time curtime = ros::Time::now();
00146 count_ = 0;
00147
00148 for (int i = 0; i < params_.window_size_; i++)
00149 {
00150 times_[i] = curtime;
00151 seq_nums_[i] = count_;
00152 }
00153
00154 hist_indx_ = 0;
00155 }
00156
00160 void tick()
00161 {
00162 boost::mutex::scoped_lock lock(lock_);
00163
00164 count_++;
00165 }
00166
00167 virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
00168 {
00169 boost::mutex::scoped_lock lock(lock_);
00170 ros::Time curtime = ros::Time::now();
00171 int curseq = count_;
00172 int events = curseq - seq_nums_[hist_indx_];
00173 double window = (curtime - times_[hist_indx_]).toSec();
00174 double freq = events / window;
00175 seq_nums_[hist_indx_] = curseq;
00176 times_[hist_indx_] = curtime;
00177 hist_indx_ = (hist_indx_ + 1) % params_.window_size_;
00178
00179 if (events == 0)
00180 {
00181 stat.summary(2, "No events recorded.");
00182 }
00183 else if (freq < *params_.min_freq_ * (1 - params_.tolerance_))
00184 {
00185 stat.summary(1, "Frequency too low.");
00186 }
00187 else if (freq > *params_.max_freq_ * (1 + params_.tolerance_))
00188 {
00189 stat.summary(1, "Frequency too high.");
00190 }
00191 else
00192 {
00193 stat.summary(0, "Desired frequency met");
00194 }
00195
00196 stat.addf("Events in window", "%d", events);
00197 stat.addf("Events since startup", "%d", count_);
00198 stat.addf("Duration of window (s)", "%f", window);
00199 stat.addf("Actual frequency (Hz)", "%f",freq);
00200 if (*params_.min_freq_ == *params_.max_freq_)
00201 stat.addf("Target frequency (Hz)", "%f",*params_.min_freq_);
00202 if (*params_.min_freq_ > 0)
00203 stat.addf("Minimum acceptable frequency (Hz)", "%f",
00204 *params_.min_freq_ * (1 - params_.tolerance_));
00205
00206 #ifdef _WIN32
00207 if (isfinite(*params_.max_freq_))
00208 #else
00209 if (finite(*params_.max_freq_))
00210 #endif
00211 stat.addf("Maximum acceptable frequency (Hz)", "%f",
00212 *params_.max_freq_ * (1 + params_.tolerance_));
00213 }
00214 };
00215
00221 struct TimeStampStatusParam
00222 {
00227 TimeStampStatusParam(const double min_acceptable = -1, const double max_acceptable = 5) :
00228 max_acceptable_(max_acceptable), min_acceptable_(min_acceptable)
00229 {}
00230
00235 double max_acceptable_;
00236
00241 double min_acceptable_;
00242
00243 };
00244
00250 static TimeStampStatusParam DefaultTimeStampStatusParam = TimeStampStatusParam();
00251
00263 class TimeStampStatus : public DiagnosticTask
00264 {
00265 private:
00266 void init()
00267 {
00268 early_count_ = 0;
00269 late_count_ = 0;
00270 zero_count_ = 0;
00271 zero_seen_ = false;
00272 max_delta_ = 0;
00273 min_delta_ = 0;
00274 deltas_valid_ = false;
00275 }
00276
00277 public:
00282 TimeStampStatus(const TimeStampStatusParam ¶ms, std::string name) :
00283 DiagnosticTask(name),
00284 params_(params)
00285 {
00286 init();
00287 }
00288
00294 TimeStampStatus(const TimeStampStatusParam ¶ms) :
00295 DiagnosticTask("Timestamp Status"),
00296 params_(params)
00297 {
00298 init();
00299 }
00300
00306 TimeStampStatus() :
00307 DiagnosticTask("Timestamp Status")
00308 {
00309 init();
00310 }
00311
00319 void tick(double stamp)
00320 {
00321 boost::mutex::scoped_lock lock(lock_);
00322
00323 if (stamp == 0)
00324 {
00325 zero_seen_ = true;
00326 }
00327 else
00328 {
00329 double delta = ros::Time::now().toSec() - stamp;
00330
00331 if (!deltas_valid_ || delta > max_delta_)
00332 max_delta_ = delta;
00333
00334 if (!deltas_valid_ || delta < min_delta_)
00335 min_delta_ = delta;
00336
00337 deltas_valid_ = true;
00338 }
00339 }
00340
00348 void tick(const ros::Time t)
00349 {
00350 tick(t.toSec());
00351 }
00352
00353 virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
00354 {
00355 boost::mutex::scoped_lock lock(lock_);
00356
00357 stat.summary(0, "Timestamps are reasonable.");
00358 if (!deltas_valid_)
00359 {
00360 stat.summary(1, "No data since last update.");
00361 }
00362 else
00363 {
00364 if (min_delta_ < params_.min_acceptable_)
00365 {
00366 stat.summary(2, "Timestamps too far in future seen.");
00367 early_count_++;
00368 }
00369
00370 if (max_delta_ > params_.max_acceptable_)
00371 {
00372 stat.summary(2, "Timestamps too far in past seen.");
00373 late_count_++;
00374 }
00375
00376 if (zero_seen_)
00377 {
00378 stat.summary(2, "Zero timestamp seen.");
00379 zero_count_++;
00380 }
00381 }
00382
00383 stat.addf("Earliest timestamp delay:", "%f", min_delta_);
00384 stat.addf("Latest timestamp delay:", "%f", max_delta_);
00385 stat.addf("Earliest acceptable timestamp delay:", "%f", params_.min_acceptable_);
00386 stat.addf("Latest acceptable timestamp delay:", "%f", params_.max_acceptable_);
00387 stat.add("Late diagnostic update count:", late_count_);
00388 stat.add("Early diagnostic update count:", early_count_);
00389 stat.add("Zero seen diagnostic update count:", zero_count_);
00390
00391 deltas_valid_ = false;
00392 min_delta_ = 0;
00393 max_delta_ = 0;
00394 zero_seen_ = false;
00395 }
00396
00397 private:
00398 TimeStampStatusParam params_;
00399 int early_count_;
00400 int late_count_;
00401 int zero_count_;
00402 bool zero_seen_;
00403 double max_delta_;
00404 double min_delta_;
00405 bool deltas_valid_;
00406 boost::mutex lock_;
00407 };
00408
00415 class Heartbeat : public DiagnosticTask
00416 {
00417 public:
00422 Heartbeat() :
00423 DiagnosticTask("Heartbeat")
00424 {
00425 }
00426
00427 virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
00428 {
00429 stat.summary(0, "Alive");
00430 }
00431 };
00432 };
00433
00434 #endif