var_buffer.h

Go to the documentation of this file.

#ifndef __XBOT_MATLOGGER2_BUFFER_BLOCK__
#define __XBOT_MATLOGGER2_BUFFER_BLOCK__

#include <string>
#include <memory>

#include <eigen3/Eigen/Dense>

namespace XBot 
{

    class VariableBuffer 
    {
    
    public:
        
        enum class Mode
        {
            // producer-consumer mode (possibly dual thread):
            // the user (or the provided MatAppender)
            // is responsible for keeping the buffer free                   
            producer_consumer,  
            
            // circular buffer mode (single thread)
            circular_buffer    
        };
        
        struct BufferInfo
        {
            // name of the variable that the new data refers to
            const char * variable_name;
            
            // bytes available to be flushed to disk
            int new_available_bytes;
            
            // free space available in buffer (0..1, 0 indicates full buffer)
            double variable_free_space;
        };
        
        typedef std::function<void(BufferInfo)> CallbackType;
        
        VariableBuffer(std::string name, 
                       int dim_rows, int dim_cols, 
                       int block_size);
        
        void set_on_block_available(CallbackType callback);
        
        void set_buffer_mode(VariableBuffer::Mode mode);
        
        const std::string& get_name() const;
        
        std::pair<int, int> get_dimension() const;
        
        template <typename Derived>
        bool add_elem(const Eigen::MatrixBase<Derived>& data);
        
        bool read_block(Eigen::MatrixXd& data, 
                        int& valid_elements);
        
        bool flush_to_queue();
        
        static int NumBlocks();
        
        ~VariableBuffer();
        
    private:
        
        class BufferBlock
        {
            
        public:
            
            typedef std::shared_ptr<BufferBlock> Ptr;
            
            BufferBlock();
            
            BufferBlock(int dim, int block_size);
            
            
            template <typename Derived>
            bool add(const Eigen::MatrixBase<Derived>& data);
            
            
            void reset();
            
            const Eigen::MatrixXd& get_data() const;
            
            
            int get_valid_elements() const;
            
            int get_size() const;
            int get_size_bytes() const;
            
            
        private:
            
            // current write index (also equals the number of valid elements)
            int _write_idx; 
            
            // memory for get_size() elements, stored column-wise
            Eigen::MatrixXd _buf;
            
        };
        
        // mode
        Mode _buffer_mode;
        
        // variable name
        std::string _name;
        
        // variable dimensions
        int _rows;
        int _cols;
        
        // current block
        BufferBlock::Ptr _current_block;
        
        // fifo spsc queue of blocks 
        class QueueImpl;
        std::unique_ptr<QueueImpl> _queue;
        
        // function to be called when a block is pushed into the queue
        CallbackType _on_block_available;
        
    };
    

 
}



template <typename Derived>
inline bool XBot::VariableBuffer::BufferBlock::add(const Eigen::MatrixBase<Derived>& data)
{
    // check if the block is full, and return false
    if(_write_idx == get_size())
    {
        return false;
    }

    // pointer to the _write_idx-th element (column of _buf)
    double * col_ptr = _buf.data() + _write_idx*_buf.rows();
    
    // Eigen-view on the column to be written
    Eigen::Map<Eigen::MatrixXd> elem_map(col_ptr, 
                                         data.rows(), data.cols());
    
    // cast data do double and write it to the current element
    elem_map.noalias() = data.template cast<double>();

    // increase _write_idx 
    _write_idx++;
    
    // if the block is not full, return true
    return true;
}


template <typename Derived>
inline bool XBot::VariableBuffer::add_elem(const Eigen::MatrixBase<Derived>& data)
{
    // check data size correctness
    if( data.size() != _rows*_cols )
    {
        fprintf(stderr, "Unable to add element to variable '%s': \
size does not match (%d vs %d)\n",
                _name.c_str(), (int)data.size(), _rows*_cols);
        
        return false;
    }
    
    // if current block is full, we push it into the queue, and try again
    if(!_current_block->add(data))
    {
        
        // write current block to queue
        flush_to_queue();
        
        // reset current block
        _current_block->reset();
        
        return add_elem(data);
    }
    
    return true;
}



#endif