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 "repubBuf.hpp"
#include <boost/lockfree/spsc_queue.hpp>
 
#include <chrono>
#include <iostream>
 
#include "rtctk/componentFramework/ddsCommon.hpp"
#include "rtctk/componentFramework/logger.hpp"
#include <rtctk/componentFramework/componentMetricsIf.hpp>
 
#include <numapp/thread.hpp>
 
namespace rtctk::telRepub {
 
using namespace rtctk::componentFramework;
 
template <uint16_t Q_SIZE = 300>
class PubThreadBase {
public:
    PubThreadBase(const char *topic_name, std::uint32_t queue_size) : m_logger(GetLogger()) {
        m_repub_buf =
            new RepubBuf<rtctk::componentFramework::AgnosticTopic>(topic_name, queue_size);
        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) : m_logger(GetLogger()) {
        m_repub_buf =
            new RepubBuf<rtctk::componentFramework::AgnosticTopic>(topic_name, queue_size);
        topic_name_m = topic_name;
        rcvSamples_m = 0;
        skipedSamples_m = 0;
        lastSkipedSample_m = 0;
    }
    virtual ~PubThreadBase() {
        if (skipedSamples_m > 0) {
            LOG4CPLUS_INFO_FMT(m_logger,
                               "[%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(m_logger,
                               "[%s] Received: %lu samples. No samples skipped\n",
                               topic_name_m.c_str(),
                               rcvSamples_m);
        }
        delete m_repub_buf;
    }
 
    std::string getTopicName() {
        return topic_name_m;
    }
 
    RepubBuf<rtctk::componentFramework::AgnosticTopic> *GetRepubBuf() {
        return m_repub_buf;
    }
 
protected:
    RepubBuf<rtctk::componentFramework::AgnosticTopic> *m_repub_buf;
    unsigned long rcvSamples_m;
    unsigned long skipedSamples_m;
    uint32_t lastSkipedSample_m;
    std::string topic_name_m;
    boost::mutex mut;
    boost::condition_variable cond;
 
    log4cplus::Logger m_logger;
};
 
// TBD maybe we do not need template
 
template <typename T = rtctk::componentFramework::AgnosticTopic,
          uint16_t Q_SIZE = 300,
          typename TDW = DataWriter>
class PubThread : public PubThreadBase<Q_SIZE> {
public:
    PubThread(TDW *dw,
              componentFramework::ComponentMetricsIf &metrics,
              std::uint32_t queue_size = 300)
        : PubThreadBase<Q_SIZE>(dw->get_topic()->get_name(), queue_size)
        , m_loop(true)
        , m_framecounter(0)
        , m_data_writer(dw)
        , m_metrics(metrics) {
        LOG4CPLUS_DEBUG_FMT(this->m_logger, "%s", __PRETTY_FUNCTION__);
 
        // Register thread to component metrics service
        std::string id_prefix = this->topic_name_m;
        // we need to convert to lowercase for OLDB  DataPoint (PC Id)
        std::transform(id_prefix.begin(), id_prefix.end(), id_prefix.begin(), ::tolower);
 
        m_pc_sent_samples_reg = m_metrics.AddCounter(
            &m_pc_sent_samples,
            CounterMetricInfo(id_prefix + "/dds_publisher/sent_samples",
                              "DDS samples sent for topic: " + this->topic_name_m));
 
    }  // PubThread
 
    ~PubThread() {
        LOG4CPLUS_DEBUG_FMT(this->m_logger, "%s", __PRETTY_FUNCTION__);
    }  //~PubThread
 
    void ReadQueuePub() {
        ReturnCode_t retcode = ReturnCode_t::RETCODE_OK;
        std::chrono::high_resolution_clock::time_point start_time;
        boost::unique_lock<boost::mutex> lock(this->mut);
 
        /* Register thread to component metrics service */
        std::string id_suffix = m_thread_name;
        // we need to convert to lowercase for OLDB  DataPoint (PC Id)
        std::transform(id_suffix.begin(), id_suffix.end(), id_suffix.begin(), ::tolower);
        m_metrics.AddThread(numapp::thisThread::GetThreadId(),
                            ThreadMetricInfo("dds_publishers/" + id_suffix,
                                             "[Dpub" + this->topic_name_m +
                                                 "] Telemetry republisher (DDS) publisher thread"));
 
        while (m_loop) {
            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
                          << "] sampleId mismatch last published: " << m_last_sample_id
                          << " current to be published: " << m_sample_id << std::endl;
            }
 
            m_last_sample_id = m_sample_id;
 
            retcode = m_data_writer->write(m_msg, eprosima::fastrtps::rtps::c_InstanceHandle_Unknown);
            this->m_repub_buf->MakeFree();
 
            m_framecounter++;
            m_pc_sent_samples++;
 
            if (retcode != ReturnCode_t::RETCODE_OK) {
                if (retcode == ReturnCode_t::RETCODE_TIMEOUT) {
                    LOG4CPLUS_ERROR_FMT(this->m_logger,
                                        "[%s]SampleId: %u. DDS write timeout!",
                                        this->topic_name_m.c_str(),
                                        m_sample_id);
                } else {
                    LOG4CPLUS_ERROR_FMT(this->m_logger,
                                        "[%s]SampleId: %u. DDS failed to write. Return code: %s",
                                        this->topic_name_m.c_str(),
                                        m_sample_id,
                                        RetcodeToString(retcode).c_str());
                }
            }
 
        }  // while
 
        LOG4CPLUS_DEBUG_FMT(this->m_logger, "[%s]Flushing queue ...\n", this->topic_name_m.c_str());
        // flushing queue
        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, eprosima::fastrtps::rtps::c_InstanceHandle_Unknown);
            this->m_repub_buf->MakeFree();
 
            m_framecounter++;
            m_pc_sent_samples++;
            if ((m_framecounter % 1) == 0) {
                LOG4CPLUS_DEBUG_FMT(this->m_logger,
                                    "[%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
        m_metrics.RemoveThread(numapp::thisThread::GetThreadId());
    }  // readQueuePub
 
    void SimPub() {
        ReturnCode_t retcode = ReturnCode_t::RETCODE_OK;
        unsigned int sampleId = 0;
        /* Register thread to component metrics service */
        std::string id_suffix = m_thread_name;
        // we need to convert to lowercase for OLDB  DataPoint (PC Id)
        std::transform(id_suffix.begin(), id_suffix.end(), id_suffix.begin(), ::tolower);
        m_metrics.AddThread(
            numapp::thisThread::GetThreadId(),
            ThreadMetricInfo("dds_publishers/" + id_suffix,
                             "[Spub" + this->topic_name_m +
                                 "] Telemetry republisher (DDS) publisher thread - SIMULATION"));
 
        while (m_loop) {
            m_message.sample_id(sampleId);
            // TBD: added some load to the sample
            retcode = m_data_writer->write(&m_message, eprosima::fastrtps::rtps::c_InstanceHandle_Unknown);
            m_framecounter++;
            m_pc_sent_samples++;
 
            if (retcode != ReturnCode_t::RETCODE_OK) {
                if (retcode == ReturnCode_t::RETCODE_TIMEOUT) {
                    LOG4CPLUS_ERROR_FMT(this->m_logger,
                                        "[%s] Timeout while sending (write): %lu sample.\n",
                                        this->topic_name_m.c_str(),
                                        m_framecounter);
                } else {
                    LOG4CPLUS_ERROR_FMT(
                        this->m_logger,
                        "[%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
        m_metrics.RemoveThread(numapp::thisThread::GetThreadId());
    }  // SimPub
 
    void CreateSimThread(u_int16_t f, numapp::NumaPolicies const &pub_thread_policies) {
        m_sim_loop_sleep = 1000000 / f;
        m_thread_name = "Spub" + this->topic_name_m;
        if (m_thread_name.size() > 15) {
            m_thread_name.erase(15);
        }
        m_thread = numapp::MakeThread(
            m_thread_name, pub_thread_policies, &PubThread<T, Q_SIZE, TDW>::SimPub, this);
    }  // CreateSimThread
 
    void CreateThread(numapp::NumaPolicies const &pub_thread_policies) {
        m_thread_name = "Dpub" + this->topic_name_m;
        if (m_thread_name.size() > 15) {
            m_thread_name.erase(15);
        }
        m_thread = numapp::MakeThread(
            m_thread_name, pub_thread_policies, &PubThread<T, Q_SIZE, TDW>::ReadQueuePub, this);
    }  // CreateThread
 
    void JoinThread() {
        m_loop = false;
        this->m_repub_buf->ReleaseWait();
        this->cond.notify_one();
        m_thread.join();
    }  // JoinThread
 
protected:
    std::thread m_thread;
    std::string m_thread_name;
    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;
    T *m_msg;
    TDW *m_data_writer;
 
    componentFramework::ComponentMetricsIf &m_metrics;
 
    // @note: OLDB does not support uint64_t which we really want, so we have to use signed 64.
    perfc::CounterI64 m_pc_sent_samples;
    perfc::ScopedRegistration m_pc_sent_samples_reg;
 
    useconds_t m_sim_loop_sleep;
};
 
}  // namespace rtctk::telRepub
 
#endif