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 "numaUtils.h"
 
#include <boost/thread/thread.hpp>
//#include <boost/lockfree/queue.hpp>
#include <boost/lockfree/spsc_queue.hpp>
#include <chrono>
#include <iostream>
 
#include "rtctk/componentFramework/logger.hpp"
 
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wregister"
 
#include "agnostictopicif.h"
#include "agnostictopicifSupport.h"
#include <ndds/ndds_cpp.h>
#include <ndds/ndds_namespace_cpp.h>
 
#pragma GCC diagnostic pop
 
using namespace std;
using namespace std::chrono;
 
using namespace rtctk::componentFramework; // for logger?
 
namespace rtctk::telRepub  {
 
uint32_t const MAX_TOPIC_SIZE = 55000; //  to be on safe side not to exceed 64k
 
struct topicT
{
    std::array<unsigned char, MAX_TOPIC_SIZE> data;
    std::size_t size;
    uint32_t sampleId;
};
 
template <int Q_SIZE=3000>
class PubThreadBase
{
public:
    PubThreadBase(const char *topic_name)
{
 
        queue_m = new boost::lockfree::spsc_queue<topicT, boost::lockfree::capacity<Q_SIZE> >;
        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);
        delete queue_m;
    }
 
    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;
    }
    //inline bool pop(T &value){ return queue_m.pop(value); }
 
    std::string getTopicName(){ return topic_name_m; }
 
protected:
    bool push_ret;
    boost::lockfree::spsc_queue<topicT, boost::lockfree::capacity<Q_SIZE> > *queue_m;
    unsigned long rcvSamples_m;
    unsigned long skipedSamples_m;
    uint32_t lastSkipedSample_m;
    std::string topic_name_m;
    boost::mutex mut;
    boost::condition_variable cond;
};
 
 
template <typename T=rtctk::AgnosticTopic, int Q_SIZE=3000>
class PubThread : public PubThreadBase<Q_SIZE>
{
public:
    PubThread(typename T::DataWriter *dw) : PubThreadBase<Q_SIZE>(dw->get_topic()->get_name())
    {
        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()
    {
        DDS::ReturnCode_t retcode;
        high_resolution_clock::time_point start_time;
//      duration<double> elapsed_time;
        //boost::mutex::scoped_lock lock(this->mut);
        boost::unique_lock<boost::mutex> lock(this->mut);
        topicT value;
 
        while(m_loop)
        {
            this->cond.wait(lock);
            while (this->queue_m->pop(value))
            {
                start_time = high_resolution_clock::now();
                // copy (should be done in specialization function
//              m_message.serialized_data.from_array(value.data.data(), value.data.size());
                m_message.serialized_data.loan_contiguous(value.data.data(), value.size, value.data.max_size());
                m_message.sample_id = value.sampleId;
                retcode = m_data_writer->write(m_message, DDS::HANDLE_NIL);
 
                m_message.serialized_data.unloan();     
 
                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(), value.sampleId);
                    }else
                    {
                        LOG4CPLUS_ERROR_FMT(GetLogger(), "[%s]SampleId: %u. DDS failed to write. Return code: %d", 
                        this->topic_name_m.c_str(), value.sampleId, retcode);
                    }
                }
 
                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(), value.sampleId,
                                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(), value.sampleId);
                    //printNUMAmem(this->queue_m, "Queue");
                }
/*
                elapsed_time = high_resolution_clock::now()-start_time;
                while (elapsed_time.count() <  0.001)
                {
                    elapsed_time = high_resolution_clock::now()-start_time;
                }
*/
 
            }//while
 
 
        }//while
 
        cout << "[" << this->topic_name_m << "] Flushing queue ..." << endl;
        //flushing queue
        while (this->queue_m->pop(value))
        {
            //m_message.serialized_data.from_array(value.data.data(), value.data.size());
            m_message.serialized_data.loan_contiguous(value.data.data(), value.size, value.data.max_size());
            m_message.sample_id = value.sampleId;
            retcode = m_data_writer->write(m_message, DDS::HANDLE_NIL);
            m_message.serialized_data.unloan();     
            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(), value.sampleId, this->skipedSamples_m,
                        static_cast<double>(this->skipedSamples_m)/this->rcvSamples_m,
                        this->lastSkipedSample_m
                        );
                //printNUMAmem(this->queue_m, "Queue");
            }
        }//while
 
    }//readQueuePub
 
    void SimPub()
    {
        DDS::ReturnCode_t retcode;
        unsigned int sampleId=0;
        while(m_loop)
        {
            m_message.sample_id = sampleId;
            //TBD: added some load to the sample
            retcode = m_data_writer->write(m_message, DDS::HANDLE_NIL);
 
            m_framecounter++;
 
            if( retcode != DDS::RETCODE_OK) {
                if (retcode==DDS::RETCODE_TIMEOUT)
                {
                    std::cerr << "[" << this->topic_name_m << "] Timeout while sending " <<  m_framecounter << " sample" << std::endl;
                }else
                {
                        std::cerr << "[" << this->topic_name_m << "] Failed to send " <<  m_framecounter << " sample return code: " << retcode << std::endl;
                }
            }//if
 
            sampleId++;
            usleep(m_simLoopSleep);
        }//while
    }//SimPub
 
    void CreateSimThread(u_int16_t f) {
        m_simLoopSleep = 1000000/f;
        m_thread = boost::thread(&PubThread<T, Q_SIZE>::SimPub, this);
        std::string thrName="Spub"+this->topic_name_m;
        pthread_setname_np(this->m_thread.native_handle(), thrName.c_str());
    }
 
    void CreateThread() {
        m_thread = boost::thread(&PubThread<T, Q_SIZE>::ReadQueuePub, this);
        std::string thrName="Dpub"+this->topic_name_m;
        pthread_setname_np(this->m_thread.native_handle(), thrName.c_str());
    }
 
    void JoinThread() { m_loop =false;  this->cond.notify_one(); m_thread.join(); }
 
    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_ERROR_FMT(GetLogger(), "   CPU affinity for thread: %s: %s", thr_name, af.c_str()); 
    }//GetAffinity
 
    
    void WaitForAcknowledgments()
    {
        cout << "wait_for_acknowledgments" << endl;
        DDS::Duration_t to = {10, 0}; //10s
        DDS::ReturnCode_t ret = m_data_writer->wait_for_acknowledgments(to);
        if( ret != DDS::RETCODE_OK) {
            std::cerr << " !Failed while waiting for acknowledgment of "
                    << "data being received by subscriptions, some data "
                    << "may not have been delivered." << std::endl;
        }//if
    }//wait_for_acknowledgments
 
protected:
    boost::thread m_thread;
    bool m_loop;
    unsigned long m_framecounter;
 
    T m_message;
    typename T::DataWriter *m_data_writer;
 
    useconds_t m_simLoopSleep;
};
 
} // closing namespace
 
#endif