computationBase.hpp

Go to the documentation of this file.

 
#ifndef RTCTK_DATATASK_COMPUTATIONBASE_HPP
#define RTCTK_DATATASK_COMPUTATIONBASE_HPP
 
#include <rtctk/componentFramework/componentMetricsIf.hpp>
#include <rtctk/componentFramework/exceptions.hpp>
#include <rtctk/componentFramework/logger.hpp>
#include <rtctk/componentFramework/serviceContainer.hpp>
 
#include <ipcq/reader.hpp>
#include <numapp/numapolicies.hpp>
#include <numapp/thread.hpp>
 
#include <fmt/format.h>
 
#include <atomic>
#include <chrono>
#include <optional>
#include <shared_mutex>
#include <thread>
 
namespace rtctk::dataTask {
 
namespace {
 
class ComputationMonitor {
public:
    struct Stats {
        size_t num_cycles;
        size_t num_samples;
        uint32_t last_sample_id;
        float freq_estimate;
        float buffer_occupancy;
        std::chrono::duration<double, std::micro> dur_read;
        std::chrono::duration<double, std::micro> dur_compute;
        std::chrono::duration<double, std::micro> dur_publish;
    };
 
    using ComponentMetricsIf = rtctk::componentFramework::ComponentMetricsIf;
 
    explicit ComputationMonitor(ComponentMetricsIf& metrics)
        : m_start_time(std::chrono::steady_clock::now()) {
        using namespace rtctk::componentFramework;
 
        m_pc_cycles_reg = metrics.AddCounter(
            &m_pc_cycles, CounterMetricInfo("num_cycles", "cycles since running"));
 
        m_pc_samples_reg = metrics.AddCounter(
            &m_pc_samples, CounterMetricInfo("num_samples", "samples read since running"));
 
        m_pc_sample_id_reg = metrics.AddCounter(
            &m_pc_sample_id, CounterMetricInfo("last_sample_id", "last observed sample id"));
 
        m_pc_freq_estimate_reg =
            metrics.AddCounter(&m_pc_freq_estimate,
                               CounterMetricInfo("frequency_estimate", "frequency estimate [Hz]"));
 
        m_pc_occupancy_reg = metrics.AddCounter(
            &m_pc_occupancy, CounterMetricInfo("buffer_occupancy", "buffer occupancy [%]"));
 
        m_pc_dur_read_reg = metrics.AddCounter(
            &m_pc_dur_read, CounterMetricInfo("duration_read", "read duration [us]"));
 
        m_pc_dur_compute_reg = metrics.AddCounter(
            &m_pc_dur_compute, CounterMetricInfo("duration_compute", "compute duration [us]"));
 
        m_pc_dur_publish_reg = metrics.AddCounter(
            &m_pc_dur_publish, CounterMetricInfo("duration_publish", "publish duration [us]"));
    }
 
    void Tick(const Stats& stats) noexcept {
        using namespace std::chrono_literals;
 
        auto now_time = std::chrono::steady_clock::now();
        auto elapsed_time = now_time - m_start_time;
        if (elapsed_time > 2s) {
            m_start_time = now_time;
 
            m_pc_cycles.Store(stats.num_cycles);
            m_pc_samples.Store(stats.num_samples);
            m_pc_sample_id.Store(stats.last_sample_id);
            m_pc_freq_estimate.Store(stats.freq_estimate);
            m_pc_occupancy.Store(stats.buffer_occupancy);
            m_pc_dur_read.Store(stats.dur_read.count());
            m_pc_dur_compute.Store(stats.dur_compute.count());
            m_pc_dur_publish.Store(stats.dur_publish.count());
        }
    }
 
    void Reset() noexcept {
        m_start_time = std::chrono::steady_clock::now();
        m_pc_cycles.Store(0);
        m_pc_samples.Store(0);
        m_pc_sample_id.Store(0);
        m_pc_freq_estimate.Store(0);
        m_pc_occupancy.Store(0.0);
        m_pc_dur_read.Store(0);
        m_pc_dur_compute.Store(0);
        m_pc_dur_publish.Store(0);
    }
 
private:
    std::chrono::steady_clock::time_point m_start_time;
 
    perfc::CounterI64 m_pc_cycles;
    perfc::ScopedRegistration m_pc_cycles_reg;
 
    perfc::CounterI64 m_pc_samples;
    perfc::ScopedRegistration m_pc_samples_reg;
 
    perfc::CounterI64 m_pc_sample_id;
    perfc::ScopedRegistration m_pc_sample_id_reg;
 
    perfc::CounterDouble m_pc_freq_estimate;
    perfc::ScopedRegistration m_pc_freq_estimate_reg;
 
    perfc::CounterDouble m_pc_occupancy;
    perfc::ScopedRegistration m_pc_occupancy_reg;
 
    perfc::CounterI64 m_pc_dur_read;
    perfc::ScopedRegistration m_pc_dur_read_reg;
 
    perfc::CounterI64 m_pc_dur_compute;
    perfc::ScopedRegistration m_pc_dur_compute_reg;
 
    perfc::CounterI64 m_pc_dur_publish;
    perfc::ScopedRegistration m_pc_dur_publish_reg;
};
 
}  // namespace
 
template <typename TopicTypeX, typename ReaderType = ipcq::Reader<TopicTypeX>>
class ComputationBase {
public:
    using TopicType = TopicTypeX;
 
    enum class State : uint8_t {
        RUNNING,  // thread is reading, skipping and performing computations
        IDLE,     // thread is continuously resetting the queue, no computations
        ERROR,    // thread terminated automatically due to an error
        OFF       // thread was not yet started or terminated manually
    };
 
    using ServiceContainer = rtctk::componentFramework::ServiceContainer;
    using ComponentMetricsIf = rtctk::componentFramework::ComponentMetricsIf;
 
    ComputationBase(ServiceContainer& services,
                    const std::string& shm_name,
                    size_t to_read,
                    size_t to_skip,
                    std::chrono::milliseconds sample_timeout,
                    std::optional<numapp::NumaPolicies> thread_policies)
        : m_logger(rtctk::componentFramework::GetLogger("rtctk"))
        , m_services(services)
        , m_shm_name(shm_name)
        , m_to_read(to_read)
        , m_to_skip(to_skip)
        , m_chunk_size(std::max(1l, (std::chrono::milliseconds(500) / sample_timeout)))
        , m_sample_timeout(sample_timeout)
        , m_thread_policies(std::move(thread_policies))
        , m_command(Command::IDLE)
        , m_state(State::OFF)
        , m_cycles_to_run(0)
        , m_cycles(0) {
    }
 
    virtual ~ComputationBase() = default;
 
    void SetSamplesToRead(size_t value) {
        m_to_read.store(value, std::memory_order_relaxed);
    }
 
    size_t GetSamplesToRead() const {
        return m_to_read.load(std::memory_order_relaxed);
    }
 
    void SetSamplesToSkip(size_t value) {
        m_to_skip.store(value, std::memory_order_relaxed);
    }
 
    size_t GetSamplesToSkip() const {
        return m_to_skip.load(std::memory_order_relaxed);
    }
 
    void Spawn() {
        // clear the previous exception to be able to recover
        m_exception = nullptr;
        m_command = Command::IDLE;
        m_thread = numapp::MakeThread("Computation",
                                      m_thread_policies.value_or(numapp::NumaPolicies()),
                                      &ComputationBase::Work,
                                      this);
        while (1) {
            using namespace std::chrono_literals;
            std::this_thread::sleep_for(10ms);
 
            CheckErrors();
 
            if (GetState() != State::OFF) {
                break;
            }
        }
    }
 
    void Join() {
        m_command = Command::EXIT;
        if (m_thread.joinable()) {
            m_thread.join();
        }
    }
 
    void Run(std::optional<size_t> cycles = std::nullopt) {
        m_cycles_to_run = cycles.value_or(0);
        m_command = Command::RUN;
    }
 
    void RunOnceSync(std::optional<std::chrono::milliseconds> poll_interval = std::nullopt) {
        Run(1);
 
        while (1) {
            using namespace std::chrono_literals;
            std::this_thread::sleep_for(poll_interval.value_or(10ms));
 
            CheckErrors();
 
            if (GetState() == State::IDLE) {
                break;
            }
        }
    }
 
    void Idle() {
        m_command = Command::IDLE;
    }
 
    State GetState() const {
        auto state = m_state.load(std::memory_order_relaxed);
        auto command = m_command.load(std::memory_order_relaxed);
 
        if (state == State::IDLE and command == Command::RUN) {
            return State::RUNNING;
        } else {
            return state;
        }
    }
 
    size_t GetCycles() const {
        return m_cycles.load(std::memory_order_relaxed);
    }
 
    void CheckErrors() {
        auto lock = std::shared_lock{m_exception_mutex};
        if (m_exception) {
            std::rethrow_exception(m_exception);
        }
    }
 
protected:
    virtual void OnThreadStart() {
    }
 
    virtual void OnCycleStart(size_t to_read) {
    }
 
    virtual void CopyData(size_t sample_idx, const TopicType& sample) noexcept = 0;
 
    virtual void Compute() = 0;
 
    virtual void Publish() = 0;
 
private:
    void Work() {
        using namespace rtctk::componentFramework;
        using namespace std::chrono_literals;
 
        const std::error_code ok{};
        std::error_code ret = ok;
 
        size_t cycles = 0;   // since State::RUNNING
        size_t samples = 0;  // since State::RUNNING
        m_cycles.store(cycles, std::memory_order_relaxed);
 
        auto t0 = std::chrono::steady_clock::now();
        auto t1 = t0;
        auto t2 = t0;
        auto t3 = t0;
        auto t4 = t0;
 
        try {
            auto reader = ReaderType::MakeReader(m_shm_name.c_str(), 30s);
 
            ComputationMonitor monitor{m_services.Get<ComponentMetricsIf>()};
 
            OnThreadStart();
 
            while (1) {
                Command command = m_command.load(std::memory_order_relaxed);
                if (command == Command::EXIT) {
                    monitor.Reset();
                    m_state.store(State::OFF, std::memory_order_relaxed);
                    return;
                } else if (command == Command::IDLE) {
                    cycles = 0;
                    samples = 0;
                    m_cycles.store(cycles, std::memory_order_relaxed);
 
                    // as long as we publish metrics to CII OLDB, we wont reset the monitor here
                    // monitor.Reset();
                    m_state.store(State::IDLE, std::memory_order_relaxed);
 
                    std::this_thread::sleep_for(10ms);
                } else {  // RUN
                    auto prev_state = m_state.exchange(State::RUNNING, std::memory_order_relaxed);
                    if (prev_state != State::RUNNING) {
                        // we are entering State::RUNNING for first time
                        ret = Reset(reader);
                        if (ret != ok) {
                            CII_THROW(RtctkException,
                                      fmt::format("SHM Reset failed: {}", ret.message()));
                        }
                    }
 
                    auto occupancy = Occupancy(reader);
 
                    size_t to_read = m_to_read.load(std::memory_order_relaxed);
                    size_t to_skip = m_to_skip.load(std::memory_order_relaxed);
 
                    OnCycleStart(to_read);
 
                    size_t sample_idx = 0;
                    uint32_t last_sample_id = 0;
                    t0 = std::chrono::steady_clock::now();
                    ret = Read(reader, to_read, [&](const TopicType& sample) {
                        CopyData(sample_idx, sample);
                        last_sample_id = sample.sample_id;
                        sample_idx++;
                    });
 
                    if (ret != ok) {
                        LOG4CPLUS_ERROR(
                            m_logger,
                            fmt::format(
                                "Work() Read: cycle {}, buffer occupancy {}", cycles, occupancy));
                        CII_THROW(RtctkException,
                                  fmt::format("SHM Read failed: {}", ret.message()));
                    }
 
                    if (m_command.load(std::memory_order_relaxed) != Command::RUN) {
                        // premature exit if command changed while reading
                        continue;
                    }
 
                    // all data has arrived for this cycle
                    t1 = std::chrono::steady_clock::now();
                    Compute();
                    t2 = std::chrono::steady_clock::now();
                    Publish();
                    t3 = std::chrono::steady_clock::now();
 
                    ret = Skip(reader, to_skip);
                    if (ret != ok) {
                        LOG4CPLUS_ERROR(
                            m_logger,
                            fmt::format(
                                "Work() Skip: cycle {}, buffer occupancy {}", cycles, occupancy));
                        CII_THROW(RtctkException,
                                  fmt::format("SHM Skip failed: {}", ret.message()));
                    }
 
                    t4 = std::chrono::steady_clock::now();
                    cycles++;
                    samples += to_read;
                    m_cycles.store(cycles, std::memory_order_relaxed);
 
                    monitor.Tick(
                        {cycles,
                         samples,
                         last_sample_id,
                         (to_read + to_skip) / std::chrono::duration<float>(t4 - t0).count(),
                         occupancy,
                         t1 - t0,
                         t2 - t1,
                         t3 - t2});
 
                    // after having computed requested number of cycles we Idle
                    if (size_t c2r = m_cycles_to_run.load(); c2r != 0 and c2r == cycles) {
                        Idle();
                    }
                }
            }
        } catch (...) {
            m_state.store(State::ERROR, std::memory_order_relaxed);
            std::scoped_lock lock(m_exception_mutex);
            m_exception = std::current_exception();
        }
    }
 
    template <typename Operation>
    std::error_code Read(ReaderType& reader, size_t to_read, Operation&& op) {
        using namespace std::chrono;
 
        size_t read = 0;
        const std::error_code ok;
        std::pair<std::error_code, size_t> ret;
        milliseconds time_elapsed{0};
        auto time_start = steady_clock::now();
 
        while (1) {
            if (m_command.load(std::memory_order_relaxed) != Command::RUN) {
                // premature exit, no error
                return {};
            }
 
            size_t to_read_now = std::min(m_chunk_size, to_read - read);
 
            ret = reader.Read(std::forward<Operation>(op), to_read_now, m_sample_timeout);
            if (ret.first != ok) {
                return ret.first;
            }
            read += ret.second;
            if (read == to_read) {
                return {};
            }
 
            time_elapsed = duration_cast<milliseconds>(steady_clock::now() - time_start);
            if (time_elapsed > m_sample_timeout * to_read) {
                return std::make_error_code(std::errc::timed_out);
            }
        }
    }
 
    std::error_code Skip(ReaderType& reader, size_t to_skip) {
        using namespace std::chrono;
 
        size_t skipped = 0;
        const std::error_code ok;
        std::pair<std::error_code, size_t> ret;
        milliseconds time_elapsed{0};
        auto time_start = steady_clock::now();
 
        while (1) {
            if ((to_skip == 0) or (m_command.load(std::memory_order_relaxed) != Command::RUN)) {
                // premature exit, no error
                return {};
            }
 
            size_t to_skip_now = std::min(m_chunk_size, to_skip - skipped);
 
            ret = reader.Skip(to_skip_now, m_sample_timeout);
            if (ret.first != ok) {
                return ret.first;
            }
            skipped += ret.second;
            if (skipped == to_skip) {
                return {};
            }
 
            time_elapsed = duration_cast<milliseconds>(steady_clock::now() - time_start);
            if (time_elapsed > m_sample_timeout * to_skip) {
                return std::make_error_code(std::errc::timed_out);
            }
        }
    }
 
    std::error_code Reset(ReaderType& reader) {
        auto ret = reader.Reset();
        if (ret == ipcq::Error::WouldBlock) {
            return {};
        }
        return ret;
    }
 
    float Occupancy(ReaderType& reader) {
        return 100.0 * (static_cast<float>(reader.NumAvailable()) / reader.Size());
    }
 
private:
    enum class Command : uint8_t { RUN, IDLE, EXIT };
 
    log4cplus::Logger& m_logger;
    ServiceContainer& m_services;
    std::string m_shm_name;
    std::atomic<size_t> m_to_read;
    std::atomic<size_t> m_to_skip;
    size_t m_chunk_size;
    std::chrono::milliseconds m_sample_timeout;
    std::optional<numapp::NumaPolicies> m_thread_policies;
    std::atomic<Command> m_command;
    std::atomic<State> m_state;
    std::atomic<size_t> m_cycles_to_run;
    std::atomic<size_t> m_cycles;
    std::exception_ptr m_exception = nullptr;
    std::shared_mutex m_exception_mutex;
    std::thread m_thread;
};
 
}  // namespace rtctk::dataTask
 
#endif  // RTCTK_DATATASK_COMPUTATIONBASE_HPP