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, 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 &params) :
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         //ROS_DEBUG("TICK %i", count_);
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 &params, std::string name) :
00283         DiagnosticTask(name),
00284         params_(params)
00285       {
00286         init();
00287       }
00288 
00294       TimeStampStatus(const TimeStampStatusParam &params) :
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


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Tue Mar 26 2019 03:09:44