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 "diagnostic_msgs/DiagnosticStatus.h"
42 #include <math.h>
43 
44 namespace diagnostic_updater
45 {
46 
52  {
57  FrequencyStatusParam(double *min_freq, double *max_freq, double tolerance = 0.1, int window_size = 5) :
58  min_freq_(min_freq), max_freq_(max_freq), tolerance_(tolerance), window_size_(window_size)
59  {}
60 
67  double *min_freq_;
68 
75  double *max_freq_;
76 
87  double tolerance_;
88 
93  };
94 
105  {
106  private:
108 
109  int count_;
110  std::vector <ros::Time> times_;
111  std::vector <int> seq_nums_;
113  boost::mutex lock_;
114 
115  public:
120  FrequencyStatus(const FrequencyStatusParam &params, std::string name) :
121  DiagnosticTask(name), params_(params),
122  times_(params_.window_size_), seq_nums_(params_.window_size_)
123  {
124  clear();
125  }
126 
133  DiagnosticTask("Frequency Status"), params_(params),
134  times_(params_.window_size_), seq_nums_(params_.window_size_)
135  {
136  clear();
137  }
138 
143  void clear()
144  {
145  boost::mutex::scoped_lock lock(lock_);
146  ros::Time curtime = ros::Time::now();
147  count_ = 0;
148 
149  for (int i = 0; i < params_.window_size_; i++)
150  {
151  times_[i] = curtime;
152  seq_nums_[i] = count_;
153  }
154 
155  hist_indx_ = 0;
156  }
157 
161  void tick()
162  {
163  boost::mutex::scoped_lock lock(lock_);
164  //ROS_DEBUG("TICK %i", count_);
165  count_++;
166  }
167 
169  {
170  boost::mutex::scoped_lock lock(lock_);
171  ros::Time curtime = ros::Time::now();
172  int curseq = count_;
173  int events = curseq - seq_nums_[hist_indx_];
174  double window = (curtime - times_[hist_indx_]).toSec();
175  double freq = 0;
176 
177  if (window != 0)
178  {
179  freq = events / window;
180  }
181  seq_nums_[hist_indx_] = curseq;
182  times_[hist_indx_] = curtime;
183  hist_indx_ = (hist_indx_ + 1) % params_.window_size_;
184 
185  if (events == 0)
186  {
187  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No events recorded.");
188  }
189  else if (window != 0 && freq < *params_.min_freq_ * (1 - params_.tolerance_))
190  {
191  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Frequency too low.");
192  }
193  else if (window != 0 && freq > *params_.max_freq_ * (1 + params_.tolerance_))
194  {
195  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Frequency too high.");
196  }
197  else if (window != 0)
198  {
199  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Desired frequency met");
200  }
201 
202  stat.addf("Events in window", "%d", events);
203  stat.addf("Events since startup", "%d", count_);
204  stat.addf("Duration of window (s)", "%f", window);
205  if (window != 0) {
206  stat.addf("Actual frequency (Hz)", "%f",freq);
207  }
208  if (*params_.min_freq_ == *params_.max_freq_)
209  stat.addf("Target frequency (Hz)", "%f",*params_.min_freq_);
210  if (*params_.min_freq_ > 0)
211  stat.addf("Minimum acceptable frequency (Hz)", "%f",
212  *params_.min_freq_ * (1 - params_.tolerance_));
213 
214  if (std::isfinite(*params_.max_freq_))
215  stat.addf("Maximum acceptable frequency (Hz)", "%f",
216  *params_.max_freq_ * (1 + params_.tolerance_));
217  }
218  };
219 
226  {
231  TimeStampStatusParam(const double min_acceptable = -1, const double max_acceptable = 5) :
232  max_acceptable_(max_acceptable), min_acceptable_(min_acceptable)
233  {}
234 
240 
246  };
247 
254 
267  {
268  private:
269  void init()
270  {
271  early_count_ = 0;
272  late_count_ = 0;
273  zero_count_ = 0;
274  zero_seen_ = false;
275  max_delta_ = 0;
276  min_delta_ = 0;
277  deltas_valid_ = false;
278  }
279 
280  public:
285  TimeStampStatus(const TimeStampStatusParam &params, std::string name) :
286  DiagnosticTask(name),
287  params_(params)
288  {
289  init();
290  }
291 
298  DiagnosticTask("Timestamp Status"),
299  params_(params)
300  {
301  init();
302  }
303 
310  DiagnosticTask("Timestamp Status")
311  {
312  init();
313  }
314 
322  void tick(double stamp)
323  {
324  boost::mutex::scoped_lock lock(lock_);
325 
326  if (stamp == 0)
327  {
328  zero_seen_ = true;
329  }
330  else
331  {
332  double delta = ros::Time::now().toSec() - stamp;
333 
334  if (!deltas_valid_ || delta > max_delta_)
335  max_delta_ = delta;
336 
337  if (!deltas_valid_ || delta < min_delta_)
338  min_delta_ = delta;
339 
340  deltas_valid_ = true;
341  }
342  }
343 
351  void tick(const ros::Time t)
352  {
353  tick(t.toSec());
354  }
355 
356  virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
357 
358  private:
364  double max_delta_;
365  double min_delta_;
367  boost::mutex lock_;
368  };
369 
375  {
376  public:
377  SlowTimeStampStatus(const TimeStampStatusParam& params, const std::string& name) : TimeStampStatus(params, name)
378  {}
379 
381  {}
382 
384  {}
385  };
386 
393  class Heartbeat : public DiagnosticTask
394  {
395  public:
401  DiagnosticTask("Heartbeat")
402  {
403  }
404 
406  {
407  stat.summary(0, "Alive");
408  }
409  };
410 }
411 
412 #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 Mon Feb 28 2022 22:18:16