readerThread.hpp

Go to the documentation of this file.

 
#ifndef RTCTK_DATATASK_READERTHREAD_HPP
#define RTCTK_DATATASK_READERTHREAD_HPP
 
#include <rtctk/componentFramework/exceptions.hpp>
#include <rtctk/componentFramework/logger.hpp>
#include <rtctk/componentFramework/stopToken.hpp>
#include <rtctk/dataTask/messageQueue.hpp>
#include <rtctk/dataTask/parameter.hpp>
#include <rtctk/dataTask/readerHelpers.hpp>
#include <rtctk/dataTask/request.hpp>
#include <rtctk/dataTask/semaphore.hpp>
 
#include <ipcq/reader.hpp>
#include <numapp/numapolicies.hpp>
#include <numapp/thread.hpp>
 
#include <chrono>
#include <cmath>
#include <ctime>
#include <functional>
#include <map>
#include <stdexcept>
#include <thread>
 
namespace rtctk::dataTask {
 
// TODO: this one is more high level, different file
class [[deprecated]] RequestTimedOut : public rtctk::componentFramework::RtctkException {
public:
    RequestTimedOut(const std::string& req_name)
        : rtctk::componentFramework::RtctkException("Request '" + req_name + "' timed out!") {
    }
};
 
class [[deprecated]] AsynchronousError : public rtctk::componentFramework::RtctkException {
public:
    AsynchronousError()
        : rtctk::componentFramework::RtctkException("An asynchronous error occured!") {
    }
 
    AsynchronousError(const std::string& text)
        : rtctk::componentFramework::RtctkException("An asynchronous error occured: '" + text +
                                                    "'!") {
    }
};
 
enum class [[deprecated]] ReaderMode : unsigned { Single, Continuous };
 
template <typename TopicType, template <typename> typename ReaderType = ipcq::Reader>
class [[deprecated]] ReaderThread {
public:
    ReaderThread()
        : m_logger(rtctk::componentFramework::GetLogger("rtctk"))
        , m_mode("reader_mode", ReaderMode::Continuous)
        , m_thread_name("thread_name", "readerThread")
        , m_queue_name("queue_name")
        , m_thread_affinity("thread_affinity")
        , m_samples_to_read("samples_to_read")
        , m_samples_to_skip("samples_to_skip", 0)
        , m_loop_frequency("loop_frequency")
        , m_error_margin("error_margin")
        , m_state(State::Off)
        , m_callback_on_data()
        , m_callback_init_thread() {
    }
 
    ~ReaderThread() {
        if (m_thread.joinable()) {
            Join();
        }
    }
 
    void Spawn() {
        using namespace std::chrono_literals;
 
        LOG4CPLUS_TRACE(m_logger, "ReaderThread::Spawn()");
 
        // check if callback to OnDataAvailable has been registered
        assert(m_callback_on_data);
 
        m_mode.CheckSet();
        m_queue_name.CheckSet();
        m_thread_name.CheckSet();
        m_samples_to_read.CheckSet();
        m_samples_to_skip.CheckSet();
        m_loop_frequency.CheckSet();
 
        m_mode.Lock();
        m_queue_name.Lock();
        m_thread_name.Lock();
        m_thread_affinity.Lock();
        m_loop_frequency.Lock();
 
        if (!m_error_margin.IsSet()) {
            m_error_margin.Set(1.5);
        }
        m_error_margin.Lock();
 
        auto policies = numapp::NumaPolicies();
        if (m_thread_affinity.IsSet()) {
            auto cpu_mask = numapp::Cpumask::MakeFromCpuStringAll(
                std::to_string(m_thread_affinity.Get()).c_str());
            policies.SetCpuAffinity(numapp::CpuAffinity(cpu_mask));
        }
 
        auto f = SendRequestAsync(Command::Idle);
 
        m_thread = numapp::MakeThread(m_thread_name.Get(), policies, &ReaderThread::Work, this);
 
        // SM is starting quickly, so hard-coded timeout will do
        auto status = f.wait_for(1200ms);
        if (status != std::future_status::ready) {
            CII_THROW(RequestTimedOut, m_command_text.at(Command::Idle));
        }
    }
 
    void Join() {
        using namespace std::chrono_literals;
 
        LOG4CPLUS_TRACE(m_logger, "ReaderThread::Join()");
 
        auto f = SendRequestAsync(Command::Exit);
 
        // here timeout depends on loop_frequency and sample_to_read/skip
        auto timeout =
            std::max(1200ms, detail::CalcTimeout(1, m_loop_frequency.Get(), m_error_margin.Get()));
        auto status = f.wait_for(timeout);
        m_thread.join();
 
        m_mode.Unlock();
        m_queue_name.Unlock();
        m_thread_name.Unlock();
        m_thread_affinity.Unlock();
        m_loop_frequency.Unlock();
 
        if (status != std::future_status::ready) {
            CII_THROW(RequestTimedOut, m_command_text.at(Command::Exit));
        }
    }
 
    void Run() {
        using namespace std::chrono_literals;
 
        LOG4CPLUS_TRACE(m_logger, "ReaderThread::Run()");
 
        // SM is going to state running quickly, so hard-coded timeout will do
        SendRequestSync(Command::Run, 1200ms);
    }
 
    void Idle() {
        using namespace std::chrono_literals;
 
        LOG4CPLUS_TRACE(m_logger, "ReaderThread::Idle()");
 
        // here timeout depends on loop_frequency and sample_to_read/skip
        auto timeout =
            std::max(1200ms, detail::CalcTimeout(1, m_loop_frequency.Get(), m_error_margin.Get()));
        SendRequestSync(Command::Idle, timeout);
    }
 
    void WaitUntilComputationAllowed() {
        using namespace std::chrono_literals;
        std::error_code const ok{};
        auto timeout = detail::CalcTimeout(m_samples_to_read.Get() + m_samples_to_skip.Get(),
                                           m_loop_frequency.Get(),
                                           m_error_margin.Get());
 
        auto ret = m_comp_allowed.Pend(timeout);
        if (ret != ok) {
            CII_THROW(AsynchronousError, ret.message());
        }
    }
 
    void WaitUntilComputationAllowed(rtctk::componentFramework::StopToken st) {
        using namespace std::chrono_literals;
        using namespace std::chrono;
        auto timeout = detail::CalcTimeout(m_samples_to_read.Get() + m_samples_to_skip.Get(),
                                           m_loop_frequency.Get(),
                                           m_error_margin.Get());
 
        auto time_start = steady_clock::now();
        while (true) {
            std::this_thread::sleep_for(5ms);
 
            if (st.StopRequested()) {
                LOG4CPLUS_TRACE(
                    m_logger, "ReaderThread::WaitUntilComputationAllowed() aborted via StopToken!");
                break;
            }
 
            auto ret = m_comp_allowed.TryPend();
            if (ret) {
                std::error_code const ok{};
                if (ret.value() != ok) {
                    CII_THROW(AsynchronousError, ret.value().message());
                }
                break;
            }
 
            if (duration_cast<milliseconds>(steady_clock::now() - time_start) > timeout) {
                CII_THROW(AsynchronousError, "ReaderThread::WaitUntilComputationAllowed() timeout");
            }
        }
    }
 
    void SignalComputationDone() {
        m_comp_done.Post();
    }
 
    void SetQueueName(std::string const& name) {
        // TODO is there a max lenght of the name?
        m_queue_name.Set(name);
    }
 
    void SetMode(ReaderMode mode) {
        m_mode.Set(mode);
    }
 
    void SetThreadName(std::string const& name) {
        // TODO check for max 16 characters length
        m_thread_name.Set(name);
    }
 
    void SetCpuAffinity(int affinity) {
        m_thread_affinity.Set(affinity);
    }
 
    void SetSamplesToRead(size_t value) {
        m_samples_to_read.Set(value);
    }
 
    void SetSamplesToSkip(size_t value) {
        m_samples_to_skip.Set(value);
    }
 
    void SetLoopFrequency(float value) {
        m_loop_frequency.Set(value);
    }
 
    void SetErrorMargin(float value) {
        m_error_margin.Set(value);
    }
 
    // call back registration
    // TODO: add checks
    void RegisterOnDataCallback(std::function<void(const TopicType& sample)> callback) {
        m_callback_on_data = callback;
    }
 
    void RegisterInitThreadCallback(std::function<void()> callback) {
        m_callback_init_thread = std::move(callback);
    }
 
private:
    enum class State : unsigned {
        Error,
        Off,
        Starting,
        Terminating,
        Idle,
        Reading,
        Skipping,
        Waiting,
        Dropping,
    };  //< known states of the readerThread
 
    // clang-format off
    const std::map<State, std::string> m_state_text = {
        {State::Error,       "Error"},
        {State::Off,         "Off"},
        {State::Starting,    "Starting"},
        {State::Terminating, "Terminating"},
        {State::Idle,        "Idle"},
        {State::Reading,     "Reading"},
        {State::Skipping,    "Skipping"},
        {State::Waiting,     "Waiting"},
        {State::Dropping,    "Dropping"},
    };  //< States names
    // clang-format on
 
    enum class Command : unsigned {
        None,
        Run,
        Idle,
        Exit,
    };  //< expected commands
 
    // clang-format off
    const std::map<Command, std::string> m_command_text = {
        {Command::None, "-"},
        {Command::Run,  "Run"},
        {Command::Idle, "Idle"},
        {Command::Exit, "Exit"},
    };  //< expected commands names
    // clang-format on
 
    std::future<void> SendRequestAsync(Command cmd) {
        Request<Command> req(cmd);
        auto f = req.GetReplyFuture();
        m_request_q.Post(std::move(req));
        return f;
    }
 
    template <class Rep, class Period>
    void SendRequestSync(Command cmd, std::chrono::duration<Rep, Period> timeout) {
        auto f = SendRequestAsync(cmd);
        auto status = f.wait_for(timeout);
        if (status != std::future_status::ready) {
            CII_THROW(RequestTimedOut, m_command_text.at(cmd));
        }
    }
 
    Request<Command> GetRequest() {
        return m_request_q.TryPend().value_or(Request(Command::None));
    }
 
    void Work() {
        using namespace rtctk::dataTask::detail;
        using namespace std::chrono;
        using namespace std::chrono_literals;
 
        auto req = GetRequest();
 
        m_state = State::Off;
        State next_state = State::Starting;
        State prev_state = State::Off;
 
        auto time_start = steady_clock::now();
        milliseconds time_elapsed{0};
        auto duration_skip = CalcTimeout(m_samples_to_skip.Get(), m_loop_frequency.Get(), 1.0);
        auto timeout_wait{duration_skip / 2};
        auto timeout_drop{duration_skip / 2};
 
        size_t to_read = 0;
        size_t to_skip = 0;
 
        std::error_code const ok{};
        std::error_code ret = ok;
 
        auto reader = ReaderType<TopicType>::MakeReader(m_queue_name.Get().c_str(), 30s);
 
        while (1) {
            prev_state = m_state;
            if (m_state != next_state) {
                m_state = next_state;
                LOG4CPLUS_TRACE(
                    m_logger,
                    "ReaderThread::Work() - changed state to '" << m_state_text.at(m_state) << "'");
            }
 
            switch (m_state) {
            case State::Starting: {
                try {
                    if (m_callback_init_thread) {
                        m_callback_init_thread();
                    }
                    next_state = State::Idle;
                } catch (...) {
                    ret = std::make_error_code(std::errc::timed_out);  // TODO: other code
                    next_state = State::Error;
                }
                break;
            }
 
            case State::Idle: {
                if (m_state != prev_state) {
                    req.SetReply();
                }
 
                req = GetRequest();
 
                if (req.GetPayload() == Command::Exit) {
                    next_state = State::Terminating;
                    break;
                } else if (req.GetPayload() == Command::Run) {
                    m_comp_done.Clear();
                    ret = Reset(reader);
                    next_state = ret == ok ? State::Reading : State::Error;
                    break;
                }
 
                std::this_thread::sleep_for(10ms);
                next_state = m_state;
                break;
            }
 
            case State::Reading: {
                if (m_state != prev_state) {
                    if (prev_state == State::Idle) {
                        m_comp_allowed.Clear();
                        req.SetReply();
                    }
                    to_read = m_samples_to_read.Get();
                    to_skip = m_samples_to_skip.Get();
                }
 
                req = GetRequest();
                if (req.GetPayload() == Command::Exit) {
                    next_state = State::Terminating;
                    break;
                } else if (req.GetPayload() == Command::Idle) {
                    next_state = State::Idle;
                    break;
                }
 
                // how many can we read in 1s?
                size_t to_read_now = m_loop_frequency.Get();
                // if less than 1 then read at least 1
                if (to_read_now == 0 and to_read > 0) {
                    to_read_now = 1;
                }
                // but never read more than to_read
                if (to_read_now > to_read) {
                    to_read_now = to_read;
                }
 
                ret = Read(reader,
                           m_callback_on_data,
                           to_read_now,
                           m_loop_frequency.Get(),
                           m_error_margin.Get());
 
                to_read -= to_read_now;
 
                if (ret != ok) {
                    next_state = State::Error;
                } else if (to_read == 0) {
                    if (m_mode.Get() == ReaderMode::Continuous) {
                        next_state = State::Skipping;
                    } else {
                        next_state = State::Idle;
                    }
                    m_comp_allowed.Post(ok);
 
                } else {
                    next_state = m_state;
                }
                break;
            }
 
            case State::Skipping: {
                req = GetRequest();
                if (req.GetPayload() == Command::Exit) {
                    next_state = State::Terminating;
                    break;
                } else if (req.GetPayload() == Command::Idle) {
                    next_state = State::Idle;
                    break;
                }
 
                // how many can we skip in 1s?
                size_t to_skip_now = m_loop_frequency.Get();
                // if less than 1 then read at least 1
                if (to_skip_now == 0 and to_skip > 0) {
                    to_skip_now = 1;
                }
                // but never skip more than to_skip
                if (to_skip_now > to_skip) {
                    to_skip_now = to_skip;
                }
 
                ret = Skip(reader, to_skip_now, m_loop_frequency.Get(), m_error_margin.Get());
 
                to_skip -= to_skip_now;
 
                if (ret != ok) {
                    next_state = State::Error;
                } else if (to_skip == 0) {
                    if (m_comp_done.TryWait()) {
                        next_state = State::Reading;
                    } else {
                        next_state = State::Waiting;
                    }
                } else {
                    next_state = m_state;
                }
                break;
            }
 
            case State::Waiting: {
                if (m_state != prev_state) {
                    time_start = steady_clock::now();
                }
 
                req = GetRequest();
                if (req.GetPayload() == Command::Exit) {
                    next_state = State::Terminating;
                    break;
                } else if (req.GetPayload() == Command::Idle) {
                    next_state = State::Idle;
                    break;
                }
 
                if (m_comp_done.TryWait()) {
                    next_state = State::Reading;
                    break;
                }
 
                std::this_thread::sleep_for(1ms);
 
                time_elapsed = duration_cast<milliseconds>(steady_clock::now() - time_start);
                if ((time_elapsed > timeout_wait) or (NumFree(reader) == 0)) {  // TODO: revisit
                    next_state = State::Dropping;
                } else {
                    next_state = m_state;
                }
                break;
            }
 
            case State::Dropping: {
                if (m_state != prev_state) {
                    time_start = steady_clock::now();
                }
 
                req = GetRequest();
                if (req.GetPayload() == Command::Exit) {
                    next_state = State::Terminating;
                    break;
                } else if (req.GetPayload() == Command::Idle) {
                    next_state = State::Idle;
                    break;
                }
 
                if (m_comp_done.TryWait()) {
                    ret = Reset(reader);
                    next_state = ret == ok ? State::Reading : State::Error;
                    break;
                }
 
                std::this_thread::sleep_for(1ms);
 
                time_elapsed = duration_cast<milliseconds>(steady_clock::now() - time_start);
                if (time_elapsed > timeout_drop) {
                    ret = std::make_error_code(std::errc::timed_out);
                    next_state = State::Error;
                } else {
                    next_state = m_state;
                }
                break;
            }
 
            case State::Error: {
                if (m_state != prev_state) {
                    m_comp_allowed.Post(ret);
                    LOG4CPLUS_ERROR(m_logger, "ReaderThread::Work() - " << ret.message());
                }
 
                req = GetRequest();
                if (req.GetPayload() == Command::Exit) {
                    next_state = State::Terminating;
                } else if (req.GetPayload() == Command::Idle) {
                    next_state = State::Idle;
                } else {
                    std::this_thread::sleep_for(100ms);
                    next_state = m_state;
                }
                break;
            }
 
            case State::Terminating: {
                next_state = State::Off;
                break;
            }
 
            case State::Off: {
                req.SetReply();
                return;
            }
            }
        }
    }
 
    log4cplus::Logger& m_logger;
 
    std::thread m_thread;  //< the readerThead member
 
    Parameter<ReaderMode> m_mode;          //< parameters mode
    Parameter<std::string> m_thread_name;  //< parameters thread name
    Parameter<std::string> m_queue_name;   //< parameter queue name
    Parameter<size_t> m_thread_affinity;   //< parameter cpu affinity
    Parameter<size_t> m_samples_to_read;   //< parameter samples to be read
    Parameter<size_t> m_samples_to_skip;   //< parameter samples to skip
    Parameter<float> m_loop_frequency;     //< parameter loop frequency
    Parameter<float> m_error_margin;       //< parameter timeout tolerance
 
    MessageQueue<Request<Command>> m_request_q;
    MessageQueue<std::error_code> m_comp_allowed;
 
    Semaphore m_comp_done;
 
    State m_state;
 
    std::function<void(const TopicType& sample)> m_callback_on_data;
    std::function<void()> m_callback_init_thread;
};
 
}  // namespace rtctk::dataTask
 
#endif  // RTCTK_DATATASK_READERTHREAD_HPP