GAAS

#ifndef CALLBACK_BUFFER_BLOCK_H
#define CALLBACK_BUFFER_BLOCK_H


//#include <queue>
#include <vector>
#include <deque>
#include <iostream>
#include <glog/logging.h>
using namespace std;

template <typename T>
class CallbackBufferBlock
{

public:

    CallbackBufferBlock<T>(){;}

    void onCallbackBlock(const T& msg);

    double queryLastMessageTime();

    T getLastMessage();

    void clear()
    {
        this->msgBuffer.clear();
    }

    inline int size()
    {
        return this->msgBuffer.size();
    }

    std::vector<T> getCopyVec()
    {
        std::vector<T> vec;
        int size = this->msgBuffer.size();
        for(int i=0;i<size;i++)
        {
            vec.push_back(this->msgBuffer.at(i));
        }
        return vec;
    }

    T operator[](int index)
    {
        return this->msgBuffer[index];
    }

    T at(int index)
    {
        if(index >= msgBuffer.size())
        {
            LOG(ERROR) <<"ERROR:dereference index overflow!"<<endl;
            LOG(ERROR) <<"index: "<<index<<",size:"<<msgBuffer.size()<<endl;
        }
        return this->msgBuffer[index];
    }

private:

    //std::queue<T> msgBuffer;
    std::deque<T> msgBuffer;
    //const int buffer_size = 100;
};

template <typename T>
void CallbackBufferBlock<T>::onCallbackBlock(const T& msg)
{

    this->msgBuffer.push_back(msg);
}

template <typename T>
double CallbackBufferBlock<T>::queryLastMessageTime()
{
    double ret_time = msgBuffer.at(this->msgBuffer.size()-1).header.stamp.toSec();
    cout<<"MessageTime:"<<ret_time<<endl;
    return ret_time;
}

template <typename T>
T CallbackBufferBlock<T>::getLastMessage()
{
    if (this->msgBuffer.size() > 0)
    {
        return this->msgBuffer.end();
    }
}

#endif