slow_topic_diag_update.h

// Author: Martin Pecka ( martin.pecka@cvut.cz )
// License: BSD

#ifndef DEM_GENERATION_SLOW_TOPIC_DIAG_UPDATE_H
#define DEM_GENERATION_SLOW_TOPIC_DIAG_UPDATE_H

#define private public
#include <diagnostic_updater/update_functions.h>
#undef private

#include <diagnostic_updater/diagnostic_updater.h>

namespace diagnostic_updater {

    class SlowFrequencyStatus : public FrequencyStatus {
    public:
        SlowFrequencyStatus(const FrequencyStatusParam &params) : FrequencyStatus(params) {

        }

        virtual ~SlowFrequencyStatus()
        {}

    public:
        virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
//            boost::mutex::scoped_lock lock(lock_); // this causes problems, maybe a recursive lock would be better
            ros::Time curtime = ros::Time::now();
            int curseq = count_;
            int events = curseq - seq_nums_[hist_indx_];
            double window = (curtime - times_[hist_indx_]).toSec();
            double freq = events / window;

            FrequencyStatus::run(stat);

            if (freq < *params_.min_freq_ * (1 - params_.tolerance_)) {
                stat.summary(1, "Frequency too low.");
            }
            else if (freq > *params_.max_freq_ * (1 + params_.tolerance_)) {
                stat.summary(1, "Frequency too high.");
            }
            else {
                stat.summary(0, "Desired frequency met");
            }
        }
    };

    class SlowTimeStampStatus : public TimeStampStatus {
    public:
        SlowTimeStampStatus(const TimeStampStatusParam &params) : TimeStampStatus(params) {

        }

        virtual ~SlowTimeStampStatus()
        {}

    public:
        virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
            deltas_valid_ = true;

            TimeStampStatus::run(stat);
        }
    };

    class SlowTopicDiagnostic : public CompositeDiagnosticTask {
    public:
        SlowTopicDiagnostic( std::string name, diagnostic_updater::Updater &diag,
                const diagnostic_updater::FrequencyStatusParam &freq,
                const diagnostic_updater::TimeStampStatusParam &stamp) :

                CompositeDiagnosticTask(name),
                freq_(freq), stamp_(stamp)
        {
            addTask(&freq_);
            addTask(&stamp_);

            diag.add(*this);
        }

        virtual ~SlowTopicDiagnostic()
        {}

        virtual void tick(const ros::Time &stamp)
        {
            stamp_.tick(stamp);
            freq_.tick();
        }

    protected:
        diagnostic_updater::SlowFrequencyStatus freq_;
        diagnostic_updater::SlowTimeStampStatus stamp_;
    };

}
#endif //DEM_GENERATION_SLOW_TOPIC_DIAG_UPDATE_H