CV学习日志：CV开发之环形缓冲器

         本文设计一种无锁环形Buffer，它的核心功能是让后端可以获取最新的传感器数据。  

         以ROS中结点概念来描述其功能就是，一个结点(发布结点)不断地获取传感器数据写到环形Buffer，另一个或多个结点去环形Buffer中取数据但要求每次取到的必须是最新传感器数据。

         于是可以按如下原则来实现这个环形Buffer：

         (1)当读操作比写操作快时，读操作等待直到写操作完成后，读操作才能继续：详细而言，就是写操作正在写一个单元时，读操作也要读这个单元；此时的设计模式为，读操作只能等待，直到写操作完成对该单元的写且释放了对该单元的控制权后，读操作才能继续。代码设计上就是读返回失败，可休眠一定时间后再读，如此反复，直到读成功。

         (2)当读操作比写操作慢时，读操作将跳过中间的所有单元，直接定位到最新单元读：需要注意的是，若读操作直接使用单元内存(对大内存单元进行拷贝耗时)而不是将单元拷贝出来后才进行相关运算，则这些运算必须足够快，至少在写操作完成一个环形周期，又回到当前单元时要运算结束，否则将读不到正确的数据进行运算。可根据运算时间与写操作的频率设置一个足够长的环形Buffer，以保证数据的安全性。

         将以上所述功能及使用样例封装为CirArr、CirMat、CirMatDemo三个类。

1.关于template<class Object, int nbuf0 = 9, int nchn0 = 9, int ndim0 = 6> class CirArr类

         (1)Object是单元类型、nbuf0是Buffer长度、nchn0是Buffer能同时支持的读者数量、ndim0是单例模式的单例数。

         (2)int getState()返回Buffer当前状态，只有Inited和Deinited两个状态。

         (3)int getBuf()&getChn()&getDim()：返回值意义同nbuf0&nchn0&ndim0。

         (4)bool getLatest(…)：读取最新单元，返回是否读成功。

         (5)int64 lockWritten(…)：移动到下一单元并锁定该单元，成功返回写序号，失败返回-1。

         (6)int64 unlockWritten(…)：释放当前单元使之能被读，成功返回1，失败返回-1。

         (7)bool init()：初始化Buffer，主要是激活状态和初始化数据单元，返回是否成功，子类通常需要重写/重载此函数。

         (8)bool deinit()：释放Buffer，主要欠激活状态和释放数据单元，返回是否成功，子类通常需要重写/重载此函数。

         (9)使用要点：主模块负责init和deinit，写模块进行lockWritten、writeData、unlockWritten、读模块进行getLatest和readData

         (10)是否继承：核心功能已在本类实现，继承主要是重写/重载init和deinit函数，需要继承主要有两种情形：一是单元是动态类型(即需要专门的内存分配和释放)，二是单元的初值有特定要求(即单元初值不能是零)。若单元为固定数据类型(即无需内存分配)且初值无特定要求(本类将所有单元置为零)，则无需继承。

         (11)设计原理：基于系统中断原理，int64在同一系统中访问具有原子性，于是定义int64的readPos和writePos。对于读，只有当writePos大于readPose，读才能进行且直接跳到writePos的位置读，同时将当前writePose的值赋给readPos，这就保证了永远读到最新的。对于写，每进行一次写操作就递增writePos，且写结束后才递增，这就保证了写的过程中不会被读。

2.关于template<class tp, int nbuf0 = 9, int nchn0 = 9, int ndim0 = 6> class CirMat 类

         (1)公共继承类CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>，核心是将Object具体化为cv::Mat_<tp>

         (2)重写/重载init和deinit函数以实现对基于cv::Mat的Buffer的初始化和释放

3.关于CirMatDemo类

         (1)定义三个业务类ControlThread、ReadThread、WriteThread和一个环形缓冲类CirMatUint8=CirMat<uchar,7)。

         (2)ControlThread负责初始化CirMatUint8、启动读线程、启动写线程。

         (3)WriteThread反复写CirMatUint8，每次写入有编号的图像，编号位于图像中心且编号递增。

         (4)ReadThread反复读CirMatUint8，每次读到图像后，就显示出来。

         (5)测试代码中，写速度约是读速度的10倍，CirMatUint8默认长度是7，测试结果显示读出的图像不正常，修改为更大的缓冲长度，可得到正常效果的图像。

         以下是详细代码，依赖于C++14、OpenCV4.x和Spdlog，封装为三个类：

         (1)CirArr：State+getState/getBuf/getChn/getDim+init+deinit+getLatest+lockWritten+unlockWritten+GetMe

         (2)CirMat：inherit from CirArr and rewrite/overload init&deinit&GetMe

         (3)CirMatDemo：CirMatUint8(from CirMat<uchar, 7>)+WriteThread+ReadThread+ControlThread+TestMe

#include <opencv2/opencv.hpp>
#include <spdlog/spdlog.h>
using namespace std;
using namespace cv;
#ifndef ns1970
#define ms1970 chrono::time_point_cast<chrono::milliseconds>(chrono::system_clock::now()).time_since_epoch().count()
#define us1970 chrono::time_point_cast<chrono::microseconds>(chrono::system_clock::now()).time_since_epoch().count()
#define ns1970 chrono::time_point_cast<chrono::nanoseconds>(chrono::system_clock::now()).time_since_epoch().count()
#endif

template<class Object, int nbuf0 = 9, int nchn0 = 9, int ndim0 = 6> class CirArr
{
public:
    static CirArr& GetMe(int dim = 0) { static CirArr us[ndim0]; return us[dim]; }
    void help()
    {
        spdlog::info("1.Read faster than write: read waits until timeout");
        spdlog::info("2.Read slower than write: read skips intermediary buffers and goes directly to the latest buffer(bufCount should be enough large in case writePos returns to readPos)");
    }

public:
    enum State
    {
        Deinited,
        Inited
    };

protected:
    State state = Deinited;
    const int nbuf = nbuf0;
    const int nchn = nchn0;
    const int ndim = ndim0;

protected:
    Object objects[nbuf0];
    int64 readPos[nchn0] = { 0 };
    int64 writePos = 0;

public:
    State getState() { return state; }
    int getBuf() { return nbuf; }
    int getChn() { return nchn; }
    int getDim() { return ndim; }

public:
    virtual bool init()
    {
        if (state == Inited) { spdlog::info("zero operation"); return true; }
        memset(objects, 0, sizeof(objects));
        memset(readPos, 0, sizeof(readPos));
        memset(&writePos, 0, sizeof(writePos));
        state = Inited;
        return true;
    }
    virtual bool deinit()
    {
        if (state == Deinited) { spdlog::info("zero operation"); return true; }
        memset(objects, 0, sizeof(objects));
        memset(readPos, 0, sizeof(readPos));
        memset(&writePos, 0, sizeof(writePos));
        state = Deinited;
        return true;
    }

public:
    bool getLatest(Object** object, int chnId, int msTimeout = 1000, int msSleep = 2)
    {
        if (state != Inited) { spdlog::critical("wrong state"); return false; }
        for (int64 t0 = ms1970; ms1970 - t0 < msTimeout;)
        {
            int64 availablePos = writePos;
            if (availablePos > readPos[chnId])
            {
                int64 relativePos = availablePos % nbuf;
                *object = objects + relativePos;
                readPos[chnId] = availablePos;
                return true;
            }
            this_thread::sleep_for(chrono::milliseconds(msSleep));
        }
        return false;
    }
    int64 lockWritten(Object** object)
    {
        if (state != Inited) { spdlog::critical("wrong state"); return -1; }
        int64 absolutePos = writePos;
        int64 relativePos = ++absolutePos % nbuf;
        *object = objects + relativePos;
        return absolutePos;
    }
    int64 unlockWritten(int64 absolutePos)
    {
        if (state != Inited) { spdlog::critical("wrong state"); return -1; }
        return (writePos = absolutePos);
    }
};


template<class tp, int nbuf0 = 9, int nchn0 = 9, int ndim0 = 6> class CirMat : public CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>
{
public://Subclass cannot access parent members without following code in GCC
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::state;
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::nbuf;
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::nchn;
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::ndim;
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::objects;
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::readPos;
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::writePos;
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::Deinited;
    using CirArr<Mat_<tp>, nbuf0, nchn0, ndim0>::Inited;

public:
    static CirMat& GetMe(int dim = 0) { static CirMat us[ndim0]; return us[dim]; }

public:
    bool init() { spdlog::critical("please call the other one"); return false; };
    bool init(int rows, int cols)
    {
        if (state == Inited) { spdlog::info("zero operation"); return true; }
        for (int k = 0; k < nbuf; ++k) { objects[k].create(rows, cols); objects[k] = 0; }
        memset(readPos, 0, sizeof(readPos));
        memset(&writePos, 0, sizeof(writePos));
        state = Inited;
        return true;
    }
    bool deinit()
    {
        if (state == Deinited) { spdlog::info("zero operation"); return true; }
        for (int k = 0; k < nbuf; ++k) objects[k].release();
        memset(readPos, 0, sizeof(readPos));
        memset(&writePos, 0, sizeof(writePos));
        state = Deinited;
        return true;
    }
};


class CirMatDemo
{
public:
    static void TestMe(int argc = 0, char** argv = 0)
    {
        //1.StartControl
        if (ControlThread::GetMe().start() != 0) spdlog::critical("ControlThread::GetMe().start failed");
        else spdlog::info("ControlThread::GetMe().start succeeded");

        //2.StopControl
        if (ControlThread::GetMe().stop() != 0) spdlog::critical("ControlThread::GetMe().stop failed");
        else spdlog::info("ControlThread::GetMe().stop succeeded");
    }

public:
    using CirMatUint8 = CirMat<uchar, 7>; //write is about ten times faster than read; set nbuf as 4/6/8/10/12/14 for showing different effect

public:
    class WriteThread
    {
    public:
        static WriteThread& GetMe() { static WriteThread me; return me; }

    public:
        enum State
        {
            Stopped,
            Started
        };

    protected:
        State state = Stopped;

    public:
        State getState() { return state; }

    public:
        bool start()
        {
            if (state == Started) { spdlog::info("zero operation"); return true; }
            if (CirMatUint8::GetMe().getState() != CirMatUint8::Inited) { spdlog::critical("CirMatUint8 not Inited"); return false; }
            state = Started;
            std::thread([this]()->void
                {
                    int k = 0;
                    while (1)
                    {
                        //Exit
                        if (state == Stopped) break;

                        //GetPointer
                        Mat_<uchar>* mat;
                        int64 absolutePos = CirMatUint8::GetMe().lockWritten(&mat);
                        if (absolutePos < 0) { spdlog::critical("CirMatUint8::GetMe().lockWritten failed"); continue; }

                        //OperateData
                        *mat = 0;
                        char num[20]; sprintf(num, "WritePos: %d", ++k);
                        putText(*mat, num, Point(mat->cols / 5, mat->rows >> 1), FONT_HERSHEY_PLAIN, 2, Scalar(255, 255, 255), 2, LINE_8, false);

                        //ReleasePointer
                        if (CirMatUint8::GetMe().unlockWritten(absolutePos) < 0) spdlog::critical("CirMatUint8::GetMe().unlockWritten failed");

                        //ControlFramerate
                        this_thread::sleep_for(50ms);
                    }
                }).detach();

                return true;
        }
        bool stop()
        {
            if (state == Stopped) { spdlog::info("zero operation"); return true; }
            state = Stopped;
            return true;
        }
    };


    class ReadThread
    {
    public:
        static ReadThread& GetMe() { static ReadThread me; return me; }

    public:
        enum State
        {
            Stopped,
            Started
        };

    protected:
        State state = Stopped;

    public:
        State getState() { return state; }

    public:
        bool start()
        {
            if (state == Started) { spdlog::info("zero operation"); return true; }
            if (CirMatUint8::GetMe().getState() != CirMatUint8::Inited) { spdlog::critical("CirMatUint8 not Inited"); return false; }
            state = Started;
            std::thread([this]()->void
                {
                    int k = 0;
                    while (1)
                    {
                        //Exit
                        if (state == Stopped) break;

                        //GetPointer
                        Mat_<uchar>* mat;
                        if (!CirMatUint8::GetMe().getLatest(&mat, 0, 100)) { spdlog::info("CirMatUint8::GetMe().getLatest failed"); continue; }

                        //OperateData
                        mat->rowRange(0, mat->rows / 3) = 255;
                        mat->rowRange(mat->rows * 2 / 3, mat->rows) = 255;
                        char num[20]; sprintf(num, "ReadPos: %d", ++k);
                        putText(*mat, num, Point(mat->cols * 0.6, mat->rows >> 1), FONT_HERSHEY_PLAIN, 2, Scalar(255, 255, 255), 2, LINE_8, false);

                        //ControlFramerate
                        this_thread::sleep_for(400ms);
                        cv::imshow("effect", *mat);
                        cv::waitKey(100);
                    }
                }).detach();

                return true;
        }
        bool stop()
        {
            if (state == Stopped) { spdlog::info("zero operation"); return true; }
            state = Stopped;
            return true;
        }
    };


    class ControlThread
    {
    public:
        static ControlThread& GetMe() { static ControlThread me; return me; }

    public:
        enum State
        {
            Stopped,
            BufferStarted,
            WriteStarted,
            ReadStarted,
            Started
        };

    protected:
        State state = Stopped;

    public:
        State getState() { return state; }

    public:
        bool start()
        {
            if (state > Stopped) { spdlog::info("zero operation"); return true; }

            //1.StartBuffer
            if (CirMatUint8::GetMe().init(720, 1280) == false)
            {
                spdlog::critical("CirMatUint8::GetMe().init failed");
                stop(); return false;
            }
            else state = State(state + 1);

            //2.StartWrite
            if (WriteThread::GetMe().start() == false)
            {
                spdlog::critical("WriteThread::GetMe().start failed");
                stop(); return false;
            }
            else state = State(state + 1);

            //3.StartRead
            if (ReadThread::GetMe().start() == false)
            {
                spdlog::critical("ReadThread::GetMe().start failed");
                stop(); return false;
            }
            else state = State(state + 1);

            //4.MainThread
            while (1)
            {
                static int sec = 60;
                if (sec == 0) break;
                spdlog::info("This is the main thread and finish in {}s", --sec);
                this_thread::sleep_for(1000ms);
            }

            return true;
        }
        bool stop()
        {
            bool ret = true;

            //StopRead
            if (state > ReadStarted)
            {
                if (ReadThread::GetMe().stop() != 0)
                {
                    spdlog::critical("ReadThread::GetMe().stop failed");
                    ret = false;
                }
                state = State(state - 1);
            }

            //StopRead
            if (state > WriteStarted)
            {
                if (WriteThread::GetMe().stop() != 0)
                {
                    spdlog::critical("WriteThread::GetMe().stop failed");
                    ret = false;
                }
                state = State(state - 1);
            }

            //StopBuffer
            if (state > BufferStarted)
            {
                if (CirMatUint8::GetMe().deinit() != 0)
                {
                    spdlog::critical("CirMatUint8::GetMe().deinit failed");
                    ret = false;
                }
                state = State(state - 1);
            }

            return ret;
        }
    };
};

int main(int argc = 0, char** argv = 0) { CirMatDemo::TestMe(argc, argv); return 0; }

         还存在另外一种需求是，不需要获得最新的传感器数据，而要获取指定id(如时间戳)或最接近指定id的数据。

　　  假设环形Buffer的单元是键值对，这就相当于给键，去环形Buffer中查询该键对应的值或与该键最接近的键对应的值。

         大致实现如下，也依赖于C++14和Spdlog，封装在类CirArrEx：State+getState/getBuf/getChn/getDim+init+deinit+find+findEx+write

template<class Object, int nbuf0 = 9, int nchn0 = 9, int ndim0 = 6> class CirArrEx
{    
public:
    static CirArrEx &GetMe(int dim = 0) { static CirArrEx us[ndim0]; return us[dim]; }

public:
    enum State
    {
        Deinited,
        Inited
    };

protected:
    State state = Deinited;
    const int nbuf = nbuf0;
    const int nchn = nchn0;
    const int ndim = ndim0;

protected:
    Object objects[nbuf0];
    int64 readPos[nchn0] = { 0 };
    int64 writePos = 0;

public:
    State getState() { return state; }
    int getBuf() { return nbuf; }
    int getChn() { return nchn; }
    int getDim() { return ndim; }

public:
    virtual bool init()
    {
        if (state == Inited) { spdlog::info("zero operation"); return true; }
        memset(objects, 0, sizeof(objects));
        memset(readPos, 0, sizeof(readPos));
        memset(&writePos, 0, sizeof(writePos));
        state = Inited;
        return true;
    }
    virtual bool deinit()
    {
        if (state == Deinited) { spdlog::info("zero operation"); return true; }
        memset(objects, 0, sizeof(objects));
        memset(readPos, 0, sizeof(readPos));
        memset(&writePos, 0, sizeof(writePos));
        state = Deinited;
        return true;
    }

public:
    bool find(int64 ts, Object *object)
    {
        if (state != Inited) { spdlog::critical("wrong state"); return false; }
        if (writePos < nbuf) false;
        int64 tsMinDiff = INT64_MAX;
        for (int64 k = (writePos + 1) % nbuf, kk = (writePos - 1) % nbuf; k != kk; k = k == nbuf ? 0 : ++k) //from oldest to latest
            if (objects[k].ts == ts)
            {
                *object = objects[k];
                return true;
            }
            else
            {
                int64 tsDiff = std::abs(ts - objects[k].ts);
                if (tsMinDiff > tsDiff)
                {
                    *object = objects[k];
                    tsMinDiff = tsDiff;
                }
            }
        return (tsMinDiff != INT64_MAX);
    }
    int findEx(int64 ts1, int64 ts2, Object *objects)
    {
        if (state != Inited) { spdlog::critical("wrong state"); return 0; }
        if (writePos < nbuf) true;
        int count = 0;
        for (int64 k = (writePos + 1) % nbuf, kk = (writePos - 1) % nbuf; k != kk; k = k == nbuf ? 0 : ++k) //from oldest to latest
            if (objects[k].ts >= ts1 && objects[k].ts <= ts2)
                objects[count++] = objects[k];
        return count;
    }
    bool write(Object *object)
    {
        if (state != Inited) { spdlog::critical("wrong state"); return false; }
        objects[++writePos % nbuf] = *object;
        return true;
    }
};