FrequencyReporter.cpp

Go to the documentation of this file.

#include "FrequencyReporter.h"
 
#include <algorithm>
#include <cstdint>
#include <numeric>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include "ArmarXCore/observers/variant/Variant.h"
 
namespace armarx
{
 
    FrequencyReporter::FrequencyReporter(DebugObserverInterfacePrx debugObserver,
                                         const std::string& name) :
        debugObserver(debugObserver), name(name)
    {
        task = new SimplePeriodicTask<>([&] { report(); }, 1000 / frequency);
 
        task->start();
    }
 
    FrequencyReporter::~FrequencyReporter()
    {
        if (task)
        {
            task->stop();
        }
    }
 
    void
    FrequencyReporter::add(const armarx::core::time::DateTime timestamp)
    {
        std::lock_guard g{mtx};
        timestamps.push_back(timestamp.toMicroSecondsSinceEpoch());
    }
 
    void
    FrequencyReporter::report()
    {
        ARMARX_TRACE;
        std::vector<int64_t> ts;
 
        {
            std::lock_guard g{mtx};
 
            // check if received enough messages
            if (timestamps.size() < 2)
            {
                ARMARX_TRACE;
                debugObserver->setDebugChannel(name,
                                               {{"frequency_mean", new armarx::Variant(0.F)},
                                                {"frequency_max", new armarx::Variant(0.F)},
                                                {"frequency_min", new armarx::Variant(0.F)},
                                                {"frequency_stddev", new armarx::Variant(0.F)}});
 
                return;
            }
 
            ts = timestamps;
            timestamps.clear();
        }
 
        std::adjacent_difference(ts.begin(), ts.end(), ts.begin());
 
        using Vector = Eigen::Matrix<std::int64_t, Eigen::Dynamic, 1>;
 
        // the first element is the diff between the first element and 0
        const Vector v = Eigen::Map<Vector>(ts.data() + 1, ts.size() - 1);
        const Eigen::ArrayXf dts = v.cast<float>();
 
        const float dtMean = dts.mean();
        const float dtMin = dts.minCoeff();
        const float dtMax = dts.maxCoeff();
        const float dtStddev = std::sqrt((dts - dtMean).square().sum() / (dts.size() - 1));
 
        const float frequencyMean = 1.0F / (dtMean / 1'000'000);
        const float frequencyMax = 1.0F / (dtMin / 1'000'000);
        const float frequencyMin = 1.0F / (dtMax / 1'000'000);
        const float frequencyStddev = 1.0F / (dtStddev / 1'000'000);
 
        debugObserver->setDebugChannel(
            name,
            {{"frequency_mean", new armarx::Variant(frequencyMean)},
             {"frequency_max", new armarx::Variant(frequencyMax)},
             {"frequency_min", new armarx::Variant(frequencyMin)},
             {"frequency_stddev", new armarx::Variant(frequencyStddev)}});
    }
} // namespace armarx