readerThread.hpp

Go to the documentation of this file.

 
#ifndef RTCTK_DATATASK_READERTHREAD_HPP
#define RTCTK_DATATASK_READERTHREAD_HPP
 
#include "rtctk/componentFramework/logger.hpp"
#include "rtctk/componentFramework/exceptions.hpp"
#include "rtctk/componentFramework/stopToken.hpp"
#include "rtctk/dataTask/semaphore.hpp"
#include "rtctk/dataTask/messageQueue.hpp"
#include "rtctk/dataTask/parameter.hpp"
#include "rtctk/dataTask/request.hpp"
#include "rtctk/dataTask/readerHelpers.hpp"
 
#include <numapp/numapolicies.hpp>
#include <numapp/thread.hpp>
#include <ipcq/reader.hpp>
 
#include <thread>
#include <chrono>
#include <ctime>
#include <cmath>
#include <map>
#include <functional>
#include <stdexcept>
 
 
namespace rtctk::dataTask {
// TODO: this one is more high level, different file
class RequestTimedOut : public rtctk::componentFramework::RtctkException
{
  public:
    RequestTimedOut(const std::string& req_name)
    : rtctk::componentFramework::RtctkException("Request '" + req_name + "' timed out!") {}
};
 
class 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 ReaderMode : unsigned {
    Single,
    Continuous
};
 
template<typename TopicType, template<typename> typename ReaderType=ipcq::Reader>
class ReaderThread {
    public:
 
        ReaderThread()
        : 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 rtctk::componentFramework::GetLogger;
            using namespace std::chrono_literals;
 
            LOG4CPLUS_INFO(GetLogger(), "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.2);
            }
            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);
 
            auto status = f.wait_for(1200ms);
            if (status != std::future_status::ready) {
                CII_THROW(RequestTimedOut, m_command_text.at(Command::Idle));
            }
        }
 
        void Join()
        {
            using rtctk::componentFramework::GetLogger;
            using namespace std::chrono_literals;
 
            LOG4CPLUS_INFO(GetLogger(), "ReaderThread::Join()");
 
            auto f = SendRequestAsync(Command::Exit);
 
            auto status = f.wait_for(1200ms);
            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 rtctk::componentFramework::GetLogger;
            using namespace std::chrono_literals;
 
            LOG4CPLUS_INFO(GetLogger(), "ReaderThread::Run()");
 
            SendRequestSync(Command::Run);
        }
 
 
        void Idle()
        {
            using rtctk::componentFramework::GetLogger;
            using namespace std::chrono_literals;
 
            LOG4CPLUS_INFO(GetLogger(), "ReaderThread::Idle()");
 
            SendRequestSync(Command::Idle);
        }
 
        void WaitUntilComputationAllowed()
        {
            using namespace std::chrono_literals;
            using rtctk::componentFramework::GetLogger;
            std::error_code const ok{};
            auto timeout = 1000*detail::CalcDuration(m_samples_to_read.Get(), \
                                                     m_loop_frequency.Get(), \
                                                     3.0);
 
            auto ret = m_comp_allowed.Pend(timeout);
            if(ret != ok) {
                CII_THROW(AsynchronousError,ret.message());
            }
        }
 
        void WaitUntilComputationAllowed(rtctk::componentFramework::StopToken st)
        {
            using rtctk::componentFramework::GetLogger;
            using namespace std::chrono_literals;
            using namespace std::chrono;
            auto timeout = 1000*detail::CalcDuration(m_samples_to_read.Get(), \
                                                     m_loop_frequency.Get(), \
                                                     3.0);
 
            auto time_start = system_clock::now();
            while(true) {
                std::this_thread::sleep_for(5ms);
 
                if(st.StopRequested()) {
                    LOG4CPLUS_INFO(GetLogger(),
                        "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(system_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(size_t 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
 
        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
 
        enum class Command: unsigned {
            None,
            Run,
            Idle,
            Exit,
        }; //< expected commands
 
        const std::map<Command, std::string> m_command_text = {
            {Command::None,         "-"},
            {Command::Run,          "Off"},
            {Command::Idle,         "Starting"},
            {Command::Exit,         "Terminating"},
        }; //< expected commands names
 
        std::future<void> SendRequestAsync(Command cmd) {
            Request<Command> req(cmd);
            auto f = req.GetReplyFuture();
            m_request_q.Post(std::move(req));
            return f;
        }
 
        void SendRequestSync(Command cmd) {
            using namespace std::chrono_literals;
 
            auto f = SendRequestAsync(cmd);
            auto status = f.wait_for(1200ms);
            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 rtctk::componentFramework::GetLogger;
            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 = system_clock::now();
            milliseconds time_elapsed {0};
            auto duration_skip = CalcDuration(m_samples_to_skip.Get(), m_loop_frequency.Get());
            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_INFO(GetLogger(), "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) {
                                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;
                        }
 
                        size_t to_read_now = std::min(to_read, m_loop_frequency.Get());
                        auto time_read_start = system_clock::now();
                        ret = Read(reader, \
                                   m_callback_on_data, \
                                   to_read_now, \
                                   m_loop_frequency.Get(), \
                                   m_error_margin.Get());
                        auto time_read_end = system_clock::now();
                        CalculateEstimatedFrequency(to_read_now, time_read_end - time_read_start);
                        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;
                        }
 
                        size_t to_skip_now = std::min(to_skip, m_loop_frequency.Get());
                        auto time_skip_start = system_clock::now();
                        ret = Skip(reader, to_skip_now, m_loop_frequency.Get(), m_error_margin.Get());
                        auto time_skip_end = system_clock::now();
                        CalculateEstimatedFrequency(to_skip_now, time_skip_end - time_skip_start);
 
                        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 = system_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>(system_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 = system_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>(system_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(GetLogger(), "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;
                    }
                }
            }
        }
 
        template <class Rep, class Period>
        void CalculateEstimatedFrequency(size_t count,
                                         std::chrono::duration<Rep, Period> time)
        {
            using rtctk::componentFramework::GetLogger;
            using namespace std::chrono;
            if(time.count() != 0)
            {
                m_estimated_loop_frequency = static_cast<float>(count) / \
                                         duration_cast<seconds>(time).count();
 
                if(!std::isinf(m_estimated_loop_frequency))
                {
                    //check if outside of error margin
                    if(m_estimated_loop_frequency < \
                    static_cast<float>(m_loop_frequency.Get())/m_error_margin.Get() || \
                    m_estimated_loop_frequency > \
                    static_cast<float>(m_loop_frequency.Get())*m_error_margin.Get())
                    {
                        LOG4CPLUS_WARN(GetLogger(), "ReaderThread: Measured SHM frequency: " <<\
                        m_estimated_loop_frequency << " expected: " << m_loop_frequency.Get());
                    }
                }
            }
        }
 
        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<size_t> m_loop_frequency;   //< parameter loop frequency
        Parameter<float> m_error_margin;      //< parameter timeout tollerance
 
        float m_estimated_loop_frequency;
 
        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
 
#endif