readerHelpers.hpp

Go to the documentation of this file.

 
#ifndef RTCTK_DATATASK_READERHELPERS_HPP
#define RTCTK_DATATASK_READERHELPERS_HPP
#include "rtctk/componentFramework/logger.hpp"
 
#include <chrono>
#include <system_error>
#include <cmath>
 
namespace rtctk::dataTask {
namespace detail {
 
 
std::chrono::milliseconds CalcDuration(size_t count, size_t loop_frequency, float error_margin = 1.0)
{
    using namespace std::chrono;
    return milliseconds{static_cast<int>(ceil(1000.0 * count / loop_frequency))};
}
 
std::pair<std::chrono::milliseconds, std::chrono::milliseconds> CalcTimeout(size_t count, \
                                                                            size_t loop_frequency,\
                                                                            float  error_margin = 2.0)
{
    using namespace std::chrono;
 
    int timeout_total = ceil((static_cast<float>(count)/loop_frequency)*1000)*error_margin;
    int timeout_short = ceil((static_cast<float>(1)/loop_frequency)*1000)*error_margin;
    return {milliseconds{timeout_short}, milliseconds{timeout_total}};
}
 
template <typename ReaderType, typename Operation>
std::error_code Read(ReaderType& reader, Operation&& op, size_t count, size_t loop_frequency, \
                                            float error_margin = 2.0)
{
    using rtctk::componentFramework::GetLogger;
    using namespace std::chrono;
 
    auto [timeout_short, timeout_total] = CalcTimeout(count, loop_frequency, error_margin);
 
    size_t read = 0;
    std::error_code const ok;
    std::pair<std::error_code, size_t> ret;
    milliseconds time_elapsed {0};
    auto time_start = system_clock::now();
 
    while (1) {
        ret = reader.Read(std::forward<Operation>(op), count, timeout_short);
        if(ret.first != ok) {
            LOG4CPLUS_ERROR(GetLogger(), "Reading from shm timed out: check queue is being filled");
            LOG4CPLUS_ERROR(GetLogger(), "Read: " << ret.second << " in " << timeout_short.count() << " ms");
            LOG4CPLUS_ERROR(GetLogger(), "Expected: " << count);
            return ret.first;
        }
        read += ret.second;
        if (read == count) {
            return {};
        }
 
        time_elapsed = duration_cast<milliseconds>(system_clock::now() - time_start);
        if (time_elapsed > timeout_total) {
            LOG4CPLUS_ERROR(GetLogger(), "Reading from shm timed out: check queue is being filled");
            LOG4CPLUS_ERROR(GetLogger(), "Read: " << read << " in " << time_elapsed.count() << " ms");
            LOG4CPLUS_ERROR(GetLogger(), "Expected: " << read);
            return std::make_error_code(std::errc::timed_out);
        }
    }
}
 
 
 
template <typename ReaderType>
std::error_code Skip(ReaderType& reader, size_t count, size_t loop_frequency, \
                                                     float error_margin = 2.0)
{
    using rtctk::componentFramework::GetLogger;
    using namespace std::chrono;
 
    auto [timeout_short, timeout_total] = CalcTimeout(count, loop_frequency, error_margin);
 
    size_t skipped = 0;
    std::error_code const ok;
    std::pair<std::error_code, size_t> ret;
    milliseconds time_elapsed {0};
    auto time_start = system_clock::now();
 
    while (1) {
        ret = reader.Skip(count, timeout_short);
        if(ret.first != ok) {
            LOG4CPLUS_ERROR(GetLogger(), "Reading from shm timed out: check queue is being filled");
            LOG4CPLUS_ERROR(GetLogger(), "Read: " << ret.second << " in " << timeout_short.count() << " ms");
            LOG4CPLUS_ERROR(GetLogger(), "Expected: " << count);
            return ret.first;
        }
        skipped += ret.second;
        if (skipped == count) {
            return {};
        }
 
        time_elapsed = duration_cast<milliseconds>(system_clock::now() - time_start);
        if (time_elapsed > timeout_total) {
            LOG4CPLUS_ERROR(GetLogger(), "Reading from shm timed out: check queue is being filled");
            LOG4CPLUS_ERROR(GetLogger(), "Read: " << skipped << " in " << time_elapsed.count() << " ms");
            LOG4CPLUS_ERROR(GetLogger(), "Expected: " << skipped);
            return std::make_error_code(std::errc::timed_out);
        }
    }
}
 
 
template <typename ReaderType>
std::error_code Reset(ReaderType& reader)
{
    if (reader.Size() != 0) {
        return reader.Reset();
    }
    return {};
}
 
template <typename ReaderType>
size_t NumFree(ReaderType& reader)
{
    return reader.Size() - reader.NumAvailable();
}
 
} // namespace
 
} // namespace
 
#endif