update_functions.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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 &params) : 
00120         DiagnosticTask("Frequency Status"), params_(params), 
00121         times_(params_.window_size_), seq_nums_(params_.window_size_)
00122     {
00123       clear();
00124     }
00125 
00130       void clear()
00131       {
00132         boost::mutex::scoped_lock lock(lock_);
00133         ros::Time curtime = ros::Time::now();
00134         count_ = 0;
00135 
00136         for (int i = 0; i < params_.window_size_; i++)
00137         {
00138           times_[i] = curtime;
00139           seq_nums_[i] = count_;
00140         }
00141 
00142         hist_indx_ = 0;
00143       }
00144 
00148       void tick()
00149       {
00150         boost::mutex::scoped_lock lock(lock_);
00151         //ROS_DEBUG("TICK %i", count_);
00152         count_++;
00153       }
00154 
00155       virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
00156       {
00157         boost::mutex::scoped_lock lock(lock_);
00158         ros::Time curtime = ros::Time::now();
00159         int curseq = count_;
00160         int events = curseq - seq_nums_[hist_indx_];
00161         double window = (curtime - times_[hist_indx_]).toSec();
00162         double freq = events / window;
00163         seq_nums_[hist_indx_] = curseq;
00164         times_[hist_indx_] = curtime;
00165         hist_indx_ = (hist_indx_ + 1) % params_.window_size_;
00166 
00167         if (events == 0)
00168         {
00169           stat.summary(2, "No events recorded.");
00170         }
00171         else if (freq < *params_.min_freq_ * (1 - params_.tolerance_))
00172         {
00173           stat.summary(1, "Frequency too low.");
00174         }
00175         else if (freq > *params_.max_freq_ * (1 + params_.tolerance_))
00176         {
00177           stat.summary(1, "Frequency too high.");
00178         }
00179         else
00180         {
00181           stat.summary(0, "Desired frequency met");
00182         }
00183 
00184         stat.addf("Events in window", "%d", events);
00185         stat.addf("Events since startup", "%d", count_);
00186         stat.addf("Duration of window (s)", "%f", window);
00187         stat.addf("Actual frequency (Hz)", "%f",freq);
00188         if (*params_.min_freq_ == *params_.max_freq_)
00189           stat.addf("Target frequency (Hz)", "%f",*params_.min_freq_);
00190         if (*params_.min_freq_ > 0)
00191           stat.addf("Minimum acceptable frequency (Hz)", "%f",
00192               *params_.min_freq_ * (1 - params_.tolerance_));
00193         if (finite(*params_.max_freq_))
00194           stat.addf("Maximum acceptable frequency (Hz)", "%f",
00195               *params_.max_freq_ * (1 + params_.tolerance_));
00196       }
00197   };
00198 
00204   struct TimeStampStatusParam
00205   {
00210     TimeStampStatusParam(const double min_acceptable = -1, const double max_acceptable = 5) :
00211       max_acceptable_(max_acceptable), min_acceptable_(min_acceptable)
00212     {}
00213 
00218     double max_acceptable_;
00219 
00224     double min_acceptable_;
00225 
00226   };
00227 
00233   static TimeStampStatusParam DefaultTimeStampStatusParam = TimeStampStatusParam();
00234 
00246   class TimeStampStatus : public DiagnosticTask
00247   {
00248     private:
00249       void init()
00250       {
00251         early_count_ = 0;
00252         late_count_ = 0;
00253         zero_count_ = 0;
00254         zero_seen_ = false;
00255         max_delta_ = 0;
00256         min_delta_ = 0;
00257         deltas_valid_ = false;
00258       }
00259 
00260     public:
00265       TimeStampStatus(const TimeStampStatusParam &params) : 
00266         DiagnosticTask("Timestamp Status"), 
00267         params_(params)
00268     {
00269       init();
00270     }
00271 
00276       TimeStampStatus() : 
00277         DiagnosticTask("Timestamp Status") 
00278     {
00279       init();
00280     }
00281 
00289       void tick(double stamp)
00290       {
00291         boost::mutex::scoped_lock lock(lock_);
00292 
00293         if (stamp == 0)
00294         {
00295           zero_seen_ = true;
00296         }
00297         else
00298         {
00299           double delta = ros::Time::now().toSec() - stamp;
00300 
00301           if (!deltas_valid_ || delta > max_delta_)
00302             max_delta_ = delta;
00303 
00304           if (!deltas_valid_ || delta < min_delta_)
00305             min_delta_ = delta;
00306 
00307           deltas_valid_ = true;
00308         }
00309       }
00310 
00318       void tick(const ros::Time t)
00319       {
00320         tick(t.toSec());
00321       }
00322 
00323       virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
00324       {
00325         boost::mutex::scoped_lock lock(lock_);
00326 
00327         stat.summary(0, "Timestamps are reasonable.");
00328         if (!deltas_valid_)
00329         {
00330           stat.summary(1, "No data since last update.");
00331         }
00332         else 
00333         {
00334           if (min_delta_ < params_.min_acceptable_)
00335           {
00336             stat.summary(2, "Timestamps too far in future seen.");
00337             early_count_++;
00338           }
00339 
00340           if (max_delta_ > params_.max_acceptable_)
00341           {
00342             stat.summary(2, "Timestamps too far in past seen.");
00343             late_count_++;
00344           }
00345 
00346           if (zero_seen_)
00347           {
00348             stat.summary(2, "Zero timestamp seen.");
00349             zero_count_++;
00350           }
00351         }
00352 
00353         stat.addf("Earliest timestamp delay:", "%f", min_delta_);
00354         stat.addf("Latest timestamp delay:", "%f", max_delta_);
00355         stat.addf("Earliest acceptable timestamp delay:", "%f", params_.min_acceptable_);
00356         stat.addf("Latest acceptable timestamp delay:", "%f", params_.max_acceptable_);
00357         stat.add("Late diagnostic update count:", late_count_); 
00358         stat.add("Early diagnostic update count:", early_count_); 
00359         stat.add("Zero seen diagnostic update count:", zero_count_); 
00360 
00361         deltas_valid_ = false;
00362         min_delta_ = 0;
00363         max_delta_ = 0;
00364         zero_seen_ = false;
00365       }
00366 
00367     private:
00368       TimeStampStatusParam params_;
00369       int early_count_;
00370       int late_count_;
00371       int zero_count_;
00372       bool zero_seen_;
00373       double max_delta_;
00374       double min_delta_;
00375       bool deltas_valid_;
00376       boost::mutex lock_;
00377   };
00378 
00379 };
00380 
00381 #endif


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Sun Oct 5 2014 23:27:19