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 = 0;
175 
176  if (window != 0)
177  {
178  freq = events / window;
179  }
180  seq_nums_[hist_indx_] = curseq;
181  times_[hist_indx_] = curtime;
182  hist_indx_ = (hist_indx_ + 1) % params_.window_size_;
183 
184  using diagnostic_msgs::DiagnosticStatus;
185 
186  if (events == 0)
187  {
188  stat.summary(DiagnosticStatus::ERROR, "No events recorded.");
189  }
190  else if (window != 0 && freq < *params_.min_freq_ * (1 - params_.tolerance_))
191  {
192  stat.summary(DiagnosticStatus::WARN, "Frequency too low.");
193  }
194  else if (window != 0 && freq > *params_.max_freq_ * (1 + params_.tolerance_))
195  {
196  stat.summary(DiagnosticStatus::WARN, "Frequency too high.");
197  }
198  else if (window != 0)
199  {
200  stat.summary(DiagnosticStatus::OK, "Desired frequency met");
201  }
202 
203  stat.addf("Events in window", "%d", events);
204  stat.addf("Events since startup", "%d", count_);
205  stat.addf("Duration of window (s)", "%f", window);
206  if (window != 0) {
207  stat.addf("Actual frequency (Hz)", "%f",freq);
208  }
209  if (*params_.min_freq_ == *params_.max_freq_)
210  stat.addf("Target frequency (Hz)", "%f",*params_.min_freq_);
211  if (*params_.min_freq_ > 0)
212  stat.addf("Minimum acceptable frequency (Hz)", "%f",
213  *params_.min_freq_ * (1 - params_.tolerance_));
214 
215  if (std::isfinite(*params_.max_freq_))
216  stat.addf("Maximum acceptable frequency (Hz)", "%f",
217  *params_.max_freq_ * (1 + params_.tolerance_));
218  }
219  };
220 
227  {
232  TimeStampStatusParam(const double min_acceptable = -1, const double max_acceptable = 5) :
233  max_acceptable_(max_acceptable), min_acceptable_(min_acceptable)
234  {}
235 
241 
247  };
248 
255 
268  {
269  private:
270  void init()
271  {
272  early_count_ = 0;
273  late_count_ = 0;
274  zero_count_ = 0;
275  zero_seen_ = false;
276  max_delta_ = 0;
277  min_delta_ = 0;
278  deltas_valid_ = false;
279  }
280 
281  public:
286  TimeStampStatus(const TimeStampStatusParam &params, std::string name) :
287  DiagnosticTask(name),
288  params_(params)
289  {
290  init();
291  }
292 
299  DiagnosticTask("Timestamp Status"),
300  params_(params)
301  {
302  init();
303  }
304 
311  DiagnosticTask("Timestamp Status")
312  {
313  init();
314  }
315 
323  void tick(double stamp)
324  {
325  boost::mutex::scoped_lock lock(lock_);
326 
327  if (stamp == 0)
328  {
329  zero_seen_ = true;
330  }
331  else
332  {
333  double delta = ros::Time::now().toSec() - stamp;
334 
335  if (!deltas_valid_ || delta > max_delta_)
336  max_delta_ = delta;
337 
338  if (!deltas_valid_ || delta < min_delta_)
339  min_delta_ = delta;
340 
341  deltas_valid_ = true;
342  }
343  }
344 
352  void tick(const ros::Time t)
353  {
354  tick(t.toSec());
355  }
356 
357  virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
358 
359  private:
365  double max_delta_;
366  double min_delta_;
368  boost::mutex lock_;
369  };
370 
376  {
377  public:
378  SlowTimeStampStatus(const TimeStampStatusParam& params, const std::string& name) : TimeStampStatus(params, name)
379  {}
380 
382  {}
383 
385  {}
386  };
387 
394  class Heartbeat : public DiagnosticTask
395  {
396  public:
402  DiagnosticTask("Heartbeat")
403  {
404  }
405 
407  {
408  stat.summary(0, "Alive");
409  }
410  };
411 }
412 
413 #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.
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_
SlowTimeStampStatus(const TimeStampStatusParam &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.
SlowTimeStampStatus(const TimeStampStatusParam &params, const std::string &name)
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 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 Thu Sep 17 2020 03:18:16