update_functions.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
37 #ifndef __DIAGNOSTIC_STATUS__UPDATE_FUNCTIONS_H__
38 #define __DIAGNOSTIC_STATUS__UPDATE_FUNCTIONS_H__
39 
41 #include <math.h>
42 
43 namespace diagnostic_updater
44 {
45 
51  {
56  FrequencyStatusParam(double *min_freq, double *max_freq, double tolerance = 0.1, int window_size = 5) :
57  min_freq_(min_freq), max_freq_(max_freq), tolerance_(tolerance), window_size_(window_size)
58  {}
59 
66  double *min_freq_;
67 
74  double *max_freq_;
75 
86  double tolerance_;
87 
92  };
93 
104  {
105  private:
107 
108  int count_;
109  std::vector <ros::Time> times_;
110  std::vector <int> seq_nums_;
112  boost::mutex lock_;
113 
114  public:
119  FrequencyStatus(const FrequencyStatusParam &params, std::string name) :
120  DiagnosticTask(name), params_(params),
121  times_(params_.window_size_), seq_nums_(params_.window_size_)
122  {
123  clear();
124  }
125 
132  DiagnosticTask("Frequency Status"), params_(params),
133  times_(params_.window_size_), seq_nums_(params_.window_size_)
134  {
135  clear();
136  }
137 
142  void clear()
143  {
144  boost::mutex::scoped_lock lock(lock_);
145  ros::Time curtime = ros::Time::now();
146  count_ = 0;
147 
148  for (int i = 0; i < params_.window_size_; i++)
149  {
150  times_[i] = curtime;
151  seq_nums_[i] = count_;
152  }
153 
154  hist_indx_ = 0;
155  }
156 
160  void tick()
161  {
162  boost::mutex::scoped_lock lock(lock_);
163  //ROS_DEBUG("TICK %i", count_);
164  count_++;
165  }
166 
168  {
169  boost::mutex::scoped_lock lock(lock_);
170  ros::Time curtime = ros::Time::now();
171  int curseq = count_;
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;
177  hist_indx_ = (hist_indx_ + 1) % params_.window_size_;
178 
179  if (events == 0)
180  {
181  stat.summary(2, "No events recorded.");
182  }
183  else if (freq < *params_.min_freq_ * (1 - params_.tolerance_))
184  {
185  stat.summary(1, "Frequency too low.");
186  }
187  else if (freq > *params_.max_freq_ * (1 + params_.tolerance_))
188  {
189  stat.summary(1, "Frequency too high.");
190  }
191  else
192  {
193  stat.summary(0, "Desired frequency met");
194  }
195 
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);
200  if (*params_.min_freq_ == *params_.max_freq_)
201  stat.addf("Target frequency (Hz)", "%f",*params_.min_freq_);
202  if (*params_.min_freq_ > 0)
203  stat.addf("Minimum acceptable frequency (Hz)", "%f",
204  *params_.min_freq_ * (1 - params_.tolerance_));
205 
206 #ifdef _WIN32
207  if (isfinite(*params_.max_freq_))
208 #else
209  if (finite(*params_.max_freq_))
210 #endif
211  stat.addf("Maximum acceptable frequency (Hz)", "%f",
212  *params_.max_freq_ * (1 + params_.tolerance_));
213  }
214  };
215 
222  {
227  TimeStampStatusParam(const double min_acceptable = -1, const double max_acceptable = 5) :
228  max_acceptable_(max_acceptable), min_acceptable_(min_acceptable)
229  {}
230 
236 
242 
243  };
244 
251 
264  {
265  private:
266  void init()
267  {
268  early_count_ = 0;
269  late_count_ = 0;
270  zero_count_ = 0;
271  zero_seen_ = false;
272  max_delta_ = 0;
273  min_delta_ = 0;
274  deltas_valid_ = false;
275  }
276 
277  public:
282  TimeStampStatus(const TimeStampStatusParam &params, std::string name) :
283  DiagnosticTask(name),
284  params_(params)
285  {
286  init();
287  }
288 
295  DiagnosticTask("Timestamp Status"),
296  params_(params)
297  {
298  init();
299  }
300 
307  DiagnosticTask("Timestamp Status")
308  {
309  init();
310  }
311 
319  void tick(double stamp)
320  {
321  boost::mutex::scoped_lock lock(lock_);
322 
323  if (stamp == 0)
324  {
325  zero_seen_ = true;
326  }
327  else
328  {
329  double delta = ros::Time::now().toSec() - stamp;
330 
331  if (!deltas_valid_ || delta > max_delta_)
332  max_delta_ = delta;
333 
334  if (!deltas_valid_ || delta < min_delta_)
335  min_delta_ = delta;
336 
337  deltas_valid_ = true;
338  }
339  }
340 
348  void tick(const ros::Time t)
349  {
350  tick(t.toSec());
351  }
352 
354  {
355  boost::mutex::scoped_lock lock(lock_);
356 
357  stat.summary(0, "Timestamps are reasonable.");
358  if (!deltas_valid_)
359  {
360  stat.summary(1, "No data since last update.");
361  }
362  else
363  {
364  if (min_delta_ < params_.min_acceptable_)
365  {
366  stat.summary(2, "Timestamps too far in future seen.");
367  early_count_++;
368  }
369 
370  if (max_delta_ > params_.max_acceptable_)
371  {
372  stat.summary(2, "Timestamps too far in past seen.");
373  late_count_++;
374  }
375 
376  if (zero_seen_)
377  {
378  stat.summary(2, "Zero timestamp seen.");
379  zero_count_++;
380  }
381  }
382 
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_);
390 
391  deltas_valid_ = false;
392  min_delta_ = 0;
393  max_delta_ = 0;
394  zero_seen_ = false;
395  }
396 
397  private:
403  double max_delta_;
404  double min_delta_;
406  boost::mutex lock_;
407  };
408 
415  class Heartbeat : public DiagnosticTask
416  {
417  public:
423  DiagnosticTask("Heartbeat")
424  {
425  }
426 
428  {
429  stat.summary(0, "Alive");
430  }
431  };
432 };
433 
434 #endif
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&#39;s DiagnosticStatusWrapper.
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Fills out this Task&#39;s DiagnosticStatusWrapper.
double * min_freq_
Minimum acceptable frequency.
TimeStampStatus(const TimeStampStatusParam &params, 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.
Author: Blaise Gassend.
A structure that holds the constructor parameters for the TimeStampStatus class.
FrequencyStatus(const FrequencyStatusParam &params, 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 &params)
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&#39;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 &params)
Constructs a FrequencyStatus class with the given parameters. Uses a default diagnostic task name of ...
static Time now()
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.
Wrapper for the diagnostic_msgs::DiagnosticStatus message that makes it easier to update...


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Wed Mar 27 2019 03:02:22