ddsPubThread.hpp

Go to the documentation of this file.

 
#ifndef RTCTK_REUSABLECOMPONENT_TELREPUB_DDS_PUB_THREAD_HPP_
#define RTCTK_REUSABLECOMPONENT_TELREPUB_DDS_PUB_THREAD_HPP_
 
#include <boost/thread/thread.hpp>
 
#include "repubBuf.hpp"
#include <boost/lockfree/spsc_queue.hpp>
 
#include <chrono>
#include <iostream>
 
#include "rtctk/componentFramework/ddsCommon.hpp"
#include "rtctk/componentFramework/logger.hpp"
 
using namespace rtctk::componentFramework;
 
namespace rtctk::telRepub {
 
uint32_t const MAX_TOPIC_SIZE = 2560000;  //  Large increase for Pixel topics (DOR)
 
struct topicT {
    std::array<unsigned char, MAX_TOPIC_SIZE> data;
    std::size_t size;
    uint32_t sampleId;
};
 
template <uint16_t Q_SIZE = 300>
class PubThreadBase {
public:
    PubThreadBase(const char *topic_name, std::uint32_t queue_size) {
#ifdef USE_RTI_DDS
        queue_m = new boost::lockfree::spsc_queue<topicT, boost::lockfree::capacity<Q_SIZE> >;
#else
        m_repub_buf = new RepubBuf<rtctk::AgnosticTopic>(topic_name, queue_size);
#endif
        topic_name_m = topic_name;
        rcvSamples_m = 0;
        skipedSamples_m = 0;
        lastSkipedSample_m = 0;
    }
 
    // TBD maybe merge two ctor
    PubThreadBase(const std::string &topic_name, std::uint32_t queue_size) {
#ifdef USE_RTI_DDS
        queue_m = new boost::lockfree::spsc_queue<topicT, boost::lockfree::capacity<Q_SIZE> >;
#else
        m_repub_buf = new RepubBuf<rtctk::AgnosticTopic>(topic_name, queue_size);
#endif
        topic_name_m = topic_name;
        rcvSamples_m = 0;
        skipedSamples_m = 0;
        lastSkipedSample_m = 0;
    }
    virtual ~PubThreadBase() {
        if (skipedSamples_m > 0) {
            LOG4CPLUS_INFO_FMT(GetLogger(),
                               "[%s] skipped %lu samples out of %lu ratio: %f. Last @: %u\n",
                               topic_name_m.c_str(),
                               skipedSamples_m,
                               rcvSamples_m,
                               static_cast<double>(skipedSamples_m) / rcvSamples_m,
                               lastSkipedSample_m);
        } else {
            LOG4CPLUS_INFO_FMT(GetLogger(),
                               "[%s] Received: %lu samples. No samples skipped\n",
                               topic_name_m.c_str(),
                               rcvSamples_m);
        }
#ifdef USE_RTI_DDS
        delete queue_m;
#else
        delete m_repub_buf;
#endif
    }
 
#ifdef USE_RTI_DDS
 
    inline virtual bool push(topicT const &value) {
        rcvSamples_m++;
 
        push_ret = queue_m->push(value);
        this->cond.notify_one();
        if (!push_ret) {
            skipedSamples_m++;
            LOG4CPLUS_DEBUG_FMT(
                GetLogger(),
                "[%s] SampleId: %u overrun, so far skipped %lu samples. Last @: %u\n",
                topic_name_m.c_str(),
                value.sampleId,
                skipedSamples_m,
                lastSkipedSample_m);
            lastSkipedSample_m = value.sampleId;
        }
        return push_ret;
    }
#endif
 
    std::string getTopicName() {
        return topic_name_m;
    }
 
    RepubBuf<rtctk::AgnosticTopic> *GetRepubBuf() {
#ifdef USE_RTI_DDS
        LOG4CPLUS_ERROR_FMT(GetLogger(), " Method Not implemented for RTI DDS (as it is not used)");
        return nullptr;
#else
        return m_repub_buf;
#endif
    }
 
protected:
    bool push_ret;
#ifdef USE_RTI_DDS
    boost::lockfree::spsc_queue<topicT, boost::lockfree::capacity<Q_SIZE> > *queue_m;
#else
    RepubBuf<rtctk::AgnosticTopic> *m_repub_buf;
#endif
    unsigned long rcvSamples_m;
    unsigned long skipedSamples_m;
    uint32_t lastSkipedSample_m;
    std::string topic_name_m;
    boost::mutex mut;
    boost::condition_variable cond;
};
 
// TBD maybe we do not need template
 
template <typename T = rtctk::AgnosticTopic, uint16_t Q_SIZE = 300, typename TDW = TypedDataWriter>
class PubThread : public PubThreadBase<Q_SIZE> {
public:
    PubThread(TDW *dw, std::uint32_t queue_size=300) : PubThreadBase<Q_SIZE>(dw->get_topic()->get_name(), queue_size) {
        LOG4CPLUS_DEBUG_FMT(GetLogger(), "%s", __PRETTY_FUNCTION__);
        m_data_writer = dw;
        m_framecounter = 0;
        m_loop = true;
    }
 
    ~PubThread() {
        LOG4CPLUS_DEBUG_FMT(GetLogger(), "%s", __PRETTY_FUNCTION__);
    }
 
    void ReadQueuePub() {
        ReturnCode_t retcode = DDS_RETCODE_OK;
        std::chrono::high_resolution_clock::time_point start_time;
        boost::unique_lock<boost::mutex> lock(this->mut);
#ifdef USE_RTI_DDS
        topicT value;
#endif
 
        while (m_loop) {
#ifdef USE_RTI_DDS
            this->cond.wait(lock);
            while (this->queue_m->pop(value)) {
                start_time = std::chrono::high_resolution_clock::now();
                m_message.serialized_data.loan_contiguous(
                    value.data.data(), value.size, value.data.max_size());
                m_sample_id = m_message.sample_id = value.sampleId;
                retcode = m_data_writer->write(m_message, DDS_HANDLE_NIL);
                m_message.serialized_data.unloan();
 
#else
            m_msg = this->m_repub_buf->WaitNextAvail();
            if (m_msg == nullptr) {
                break;
            }  // we assume that nullptr is returned just when ReleaseWait is called
            m_sample_id = m_msg->sample_id();
            if (m_sample_id != m_last_sample_id + 1) {
                std::cout << "[" << this->topic_name_m
                          << "] sample id mismatch last: " << m_last_sample_id
                          << " current: " << m_sample_id << std::endl;
            }
 
            m_last_sample_id = m_sample_id;
 
            retcode = m_data_writer->write(m_msg, DDS_HANDLE_NIL);
            this->m_repub_buf->MakeFree();
#endif
 
                m_framecounter++;
 
                if (retcode != DDS_RETCODE_OK) {
                    if (retcode == DDS_RETCODE_TIMEOUT) {
                        LOG4CPLUS_ERROR_FMT(GetLogger(),
                                            "[%s]SampleId: %u. DDS write timeout!",
                                            this->topic_name_m.c_str(),
                                            m_sample_id);
                    } else {
                        LOG4CPLUS_ERROR_FMT(
                            GetLogger(),
                            "[%s]SampleId: %u. DDS failed to write. Return code: %s",
                            this->topic_name_m.c_str(),
                            m_sample_id,
                            RetcodeToString(retcode).c_str());
                    }
                }
#ifdef USE_RTI_DDS
                if ((m_framecounter % 2000) == 0) {
                    if (this->skipedSamples_m > 0) {
                        LOG4CPLUS_DEBUG_FMT(
                            GetLogger(),
                            "[%s]SampleId: %u. Dropped: %lu (%f). Last @: %u\n",
                            this->topic_name_m.c_str(),
                            m_sample_id,
                            this->skipedSamples_m,
                            static_cast<double>(this->skipedSamples_m) / this->rcvSamples_m,
                            this->lastSkipedSample_m);
                    } else {
                        LOG4CPLUS_DEBUG_FMT(GetLogger(),
                                            "[%s]SampleId: %u. No Drops so far.\n",
                                            this->topic_name_m.c_str(),
                                            m_sample_id);
                    }
                }
#endif
 
#ifdef USE_RTI_DDS
            }  // while
#endif
 
        }  // while
 
        LOG4CPLUS_DEBUG_FMT(GetLogger(), "[%s]Flushing queue ...\n", this->topic_name_m.c_str());
        // flushing queue
#ifdef USE_RTI_DDS
        while (this->queue_m->pop(value)) {
            m_message.serialized_data.loan_contiguous(
                value.data.data(), value.size, value.data.max_size());
            m_sample_id = m_message.sample_id = value.sampleId;
            retcode = m_data_writer->write(m_message, DDS_HANDLE_NIL);
            m_message.serialized_data.unloan();
#else
        while (this->m_repub_buf->GetAvail() > 0) {
            m_msg = this->m_repub_buf->WaitNextAvail();
            if (m_msg == nullptr) {
                break;
            }  // we assume that nullptr is returned just when ReleaseWait is called
            m_sample_id = m_msg->sample_id();
            retcode = m_data_writer->write(m_msg, DDS_HANDLE_NIL);
            this->m_repub_buf->MakeFree();
#endif
 
            m_framecounter++;
            if ((m_framecounter % 1) == 0) {
                LOG4CPLUS_DEBUG_FMT(GetLogger(),
                                    "[%s] Flushing SampleId: %u. Droped: %lu (%f). Last @ %u\n",
                                    this->topic_name_m.c_str(),
                                    m_sample_id,
                                    this->skipedSamples_m,
                                    static_cast<double>(this->skipedSamples_m) / this->rcvSamples_m,
                                    this->lastSkipedSample_m);
                // printNUMAmem(this->queue_m, "Queue");
            }
        }  // while
 
    }  // readQueuePub
 
    void SimPub() {
        ReturnCode_t retcode = DDS_RETCODE_OK;
        unsigned int sampleId = 0;
        while (m_loop) {
#ifdef USE_RTI_DDS
            m_message.sample_id = sampleId;
            // TBD: added some load to the sample
            retcode = m_data_writer->write(m_message, DDS_HANDLE_NIL);
#else
            m_message.sample_id(sampleId);
            // TBD: added some load to the sample
            retcode = m_data_writer->write(&m_message, DDS_HANDLE_NIL);
#endif
            m_framecounter++;
 
            if (retcode != DDS_RETCODE_OK) {
                if (retcode == DDS_RETCODE_TIMEOUT) {
                    LOG4CPLUS_ERROR_FMT(GetLogger(),
                                        "[%s] Timeout while sending (write): %lu sample.\n",
                                        this->topic_name_m.c_str(),
                                        m_framecounter);
                } else {
                    LOG4CPLUS_ERROR_FMT(
                        GetLogger(),
                        "[%s] Failed to send (write): %lu sample. Return Code: %s\n",
                        this->topic_name_m.c_str(),
                        m_framecounter,
                        RetcodeToString(retcode).c_str());
                }
            }  // if
 
            sampleId++;
            usleep(m_sim_loop_sleep);
        }  // while
    }      // SimPub
 
    void CreateSimThread(u_int16_t f) {
        m_sim_loop_sleep = 1000000 / f;
        m_thread = boost::thread(&PubThread<T, Q_SIZE, TDW>::SimPub, this);
        std::string thr_name = "Spub" + this->topic_name_m;
        pthread_setname_np(this->m_thread.native_handle(), thr_name.c_str());
    }
 
    void CreateThread() {
        m_thread = boost::thread(&PubThread<T, Q_SIZE, TDW>::ReadQueuePub, this);
        std::string thr_name = "Dpub" + this->topic_name_m;
        pthread_setname_np(this->m_thread.native_handle(), thr_name.c_str());
    }
 
    void JoinThread() {
        m_loop = false;
#ifndef USE_RTI_DDS
        this->m_repub_buf->ReleaseWait();
#endif
        this->cond.notify_one();
        m_thread.join();
    }  // JoinThread
 
    void SetAffinity(unsigned int cpu) {
        int ret;
        cpu_set_t cpuset;
        CPU_ZERO(&cpuset);
        CPU_SET(cpu, &cpuset);
        ret = pthread_setaffinity_np(m_thread.native_handle(), sizeof(cpuset), &cpuset);
        if (ret != 0) {
            char thr_name[64];
            pthread_getname_np(m_thread.native_handle(), thr_name, 64);
            LOG4CPLUS_ERROR_FMT(GetLogger(),
                                "%s ERROR set thread affinity for: %s. Error: %s(%d)",
                                __PRETTY_FUNCTION__,
                                thr_name,
                                strerror(ret),
                                ret);
        }  // if
    }      // SetAffinity
 
    void PrintAffinity() {
        cpu_set_t cpuset;
        CPU_ZERO(&cpuset);
 
        pthread_getaffinity_np(m_thread.native_handle(), sizeof(cpuset), &cpuset);
 
        std::string af;
        for (int j = 0; j < CPU_SETSIZE; j++) {
            if (CPU_ISSET(j, &cpuset)) {
                af += std::to_string(j) + " ";
            }
        }
        char thr_name[64];
        pthread_getname_np(m_thread.native_handle(), thr_name, 64);
        LOG4CPLUS_INFO_FMT(GetLogger(), "   CPU affinity for thread: %s: %s", thr_name, af.c_str());
    }  // GetAffinity
 
#ifdef USE_RTI_DDS
 
    void WaitForAcknowledgments() {
        Duration_t to = {10, 0};  // 10s
        ReturnCode_t ret = m_data_writer->wait_for_acknowledgments(to);
        if (ret != DDS_RETCODE_OK) {
            LOG4CPLUS_ERROR_FMT(GetLogger(),
                                "[%s] Failed wait_for_acknowledgments. Return code: %s\n",
                                this->topic_name_m.c_str(),
                                m_framecounter,
                                RetcodeToString(ret).c_str());
        }  // if
    }      // wait_for_acknowledgments
#endif
 
protected:
    boost::thread m_thread;
    std::atomic<bool> m_loop;
    unsigned long m_framecounter;
    uint32_t m_sample_id = 0;  // TBD: can be removed, and use direct m_msg->sample_id() when needed
    uint32_t m_last_sample_id = 0;
 
    T m_message;
#ifndef USE_RTI_DDS
    T *m_msg;
#endif
    TDW *m_data_writer;
 
    useconds_t m_sim_loop_sleep;
};
 
}  // namespace rtctk::telRepub
 
#endif