RingBuffer.h

Go to the documentation of this file.

// -*- C++ -*-
#ifndef RTC_RINGBUFFER_H
#define RTC_RINGBUFFER_H
 
#include <mutex>
#include <condition_variable>
#include <coil/stringutil.h>
 
#include <rtm/BufferBase.h>
#include <rtm/BufferStatus.h>
 
#include <algorithm>
#include <iostream>
#include <string>
#include <vector>
 
#define RINGBUFFER_DEFAULT_LENGTH 8
namespace RTC
{
  template <class DataType>
  class RingBuffer
    : public BufferBase<DataType>
  {
  public:
    explicit RingBuffer(long int length = RINGBUFFER_DEFAULT_LENGTH)
      : m_length(length), m_buffer(m_length)
    {
      this->reset();
    }
 
    ~RingBuffer() override;
 
    void init(const coil::Properties& prop) override
    {
      initLength(prop);
      initWritePolicy(prop);
      initReadPolicy(prop);
    }
 
    size_t length() const override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      return m_length;
    }
 
    BufferStatus length(size_t n) override
    {
      m_buffer.resize(n);
      m_length = n;
      this->reset();
      return BufferStatus::OK;
    }
 
    BufferStatus reset() override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      m_fillcount = 0;
      m_wcount = 0;
      m_wpos = 0;
      m_rpos = 0;
      return BufferStatus::OK;
    }
 
 
 
    //----------------------------------------------------------------------
    DataType* wptr(long int n = 0) override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      return &m_buffer[(m_wpos + n + m_length) % m_length];
    }
 
    BufferStatus advanceWptr(long int n = 1, bool unlock_enable = true) override
    {
      bool empty_ = false;
      bool lock_ = (unlock_enable && n > 0);
      if(lock_)
        {
          m_empty.mutex.lock();
          empty_ = empty();
        }
      // n > 0 :
      //     n satisfies n <= writable elements
      //                 n <= m_length - m_fillcout
      // n < 0 : -n = n'
      //     n satisfies n'<= readable elements
      //                 n'<= m_fillcount
      //                 n >= - m_fillcount
      {
          std::lock_guard<std::mutex> guard(m_posmutex);
          if ((n > 0 && n > static_cast<long int>(m_length) - static_cast<long int>(m_fillcount)) ||
              (n < 0 && n < -static_cast<long int>(m_fillcount)))
            {
              if (lock_)
              {
                  m_empty.mutex.unlock();
              }
              return BufferStatus::PRECONDITION_NOT_MET;
            }
 
          m_wpos = (m_wpos + n + m_length) % m_length;
          m_fillcount += n;
          m_wcount += n;
      }
 
      if(lock_)
        {
          if(empty_)
            {
              m_empty.cond.notify_one();
            }
          m_empty.mutex.unlock();
        }
 
      return BufferStatus::OK;
    }
    BufferStatus put(const DataType& value) override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      m_buffer[m_wpos] = value;
      return BufferStatus::OK;
    }
 
    BufferStatus write(const DataType& value,
                       std::chrono::nanoseconds timeout
                       = std::chrono::nanoseconds(-1)) override
    {
      {
      std::unique_lock<std::mutex> guard(m_full.mutex);
 
      if (full())
        {
 
          bool timedwrite(m_timedwrite);
          bool overwrite(m_overwrite);
 
          if (timeout >= std::chrono::seconds::zero())  // block mode
            {
              timedwrite = true;
              overwrite  = false;
            }
 
          if (overwrite && !timedwrite)  // "overwrite" mode
            {
              advanceRptr(1,false);
            }
          else if (!overwrite && !timedwrite)  // "do_nothing" mode
            {
              return BufferStatus::FULL;
            }
          else if (!overwrite && timedwrite)  // "block" mode
            {
              if (timeout < std::chrono::seconds::zero())
                {
                  timeout = m_wtimeout;
                }
              if (std::cv_status::timeout == m_empty.cond.wait_for(guard, timeout))
                {
                  return BufferStatus::TIMEOUT;
                }
            }
          else                                    // unknown condition
            {
              return BufferStatus::PRECONDITION_NOT_MET;
            }
        }
      }
 
      put(value);
 
      advanceWptr(1);
 
 
      return BufferStatus::OK;
    }
 
    size_t writable() const override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      return m_length - m_fillcount;
    }
 
    bool full() const override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      return m_length == m_fillcount;
    }
 
    //----------------------------------------------------------------------
    DataType* rptr(long int n = 0) override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      return &(m_buffer[(m_rpos + n + m_length) % m_length]);
    }
 
    BufferStatus advanceRptr(long int n = 1, bool unlock_enable = true) override
    {
      bool full_ = false;
      bool lock_ = (unlock_enable && n > 0);
      if(lock_)
        {
          m_full.mutex.lock();
          full_ = full();
        }
      // n > 0 :
      //     n satisfies n <= readable elements
      //                 n <= m_fillcout
      // n < 0 : -n = n'
      //     n satisfies n'<= m_length - m_fillcount
      //                 n >= m_fillcount - m_length
      {
          std::lock_guard<std::mutex> guard(m_posmutex);
          if ((n > 0 && n > static_cast<long int>(m_fillcount)) ||
              (n < 0 && n < static_cast<long int>(m_fillcount) - static_cast<long int>(m_length)))
            {
              if (lock_)
              {
                  m_full.mutex.unlock();
              }
              return BufferStatus::PRECONDITION_NOT_MET;
            }
 
          m_rpos = (m_rpos + n + m_length) % m_length;
          m_fillcount -= n;
      }
 
      if(lock_)
        {
          if(full_)
            {
              m_full.cond.notify_one();
            }
          m_full.mutex.unlock();
        }
 
      return BufferStatus::OK;
    }
 
    BufferStatus get(DataType& value) override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      value = m_buffer[m_rpos];
      return BufferStatus::OK;
    }
 
 
    DataType& get() override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      return m_buffer[m_rpos];
    }
 
 
    BufferStatus read(DataType& value,
                      std::chrono::nanoseconds timeout
                      = std::chrono::nanoseconds(-1)) override
    {
      {
      std::unique_lock<std::mutex> guard(m_empty.mutex);
 
      if (empty())
        {
          bool timedread(m_timedread);
          bool readback(m_readback);
 
          if (timeout >= std::chrono::seconds::zero()) // block mode
            {
              timedread = true;
              readback  = false;
              timeout = m_rtimeout;
            }
 
          if (readback && !timedread)       // "readback" mode
            {
              if (!(m_wcount > 0))
                {
                  return BufferStatus::EMPTY;
                }
              advanceRptr(-1,false);
            }
          else if (!readback && !timedread)  // "do_nothing" mode
            {
              return BufferStatus::EMPTY;
            }
          else if (!readback && timedread)  // "block" mode
            {
              if (timeout < std::chrono::seconds::zero())
                {
                  timeout = m_rtimeout;
                }
              if (std::cv_status::timeout == m_empty.cond.wait_for(guard, timeout))
                {
                  return BufferStatus::TIMEOUT;
                }
            }
          else                                    // unknown condition
            {
              return BufferStatus::PRECONDITION_NOT_MET;
            }
        }
      }
 
      get(value);
 
      advanceRptr(1);
 
      return BufferStatus::OK;
    }
 
    size_t readable() const override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      return m_fillcount;
    }
 
    bool empty() const override
    {
      std::lock_guard<std::mutex> guard(m_posmutex);
      return m_fillcount == 0;
    }
 
  private:
    void initLength(const coil::Properties& prop)
    {
      if (!prop["length"].empty())
        {
          size_t n;
          if (coil::stringTo(n, prop["length"].c_str()))
            {
              if (n > 0)
                {
                  this->length(n);
                }
            }
        }
    }
 
    void initWritePolicy(const coil::Properties& prop)
    {
      std::string policy(coil::normalize(prop["write.full_policy"]));
      if (policy == "overwrite")
        {
          m_overwrite = true;
          m_timedwrite = false;
        }
      else if (policy == "do_nothing")
        {
          m_overwrite = false;
          m_timedwrite = false;
        }
      else if (policy == "block")
        {
          m_overwrite = false;
          m_timedwrite = true;
 
          std::chrono::nanoseconds tm;
          if (coil::stringTo(tm, prop["write.timeout"].c_str())
              && !(tm < std::chrono::seconds::zero()))
            {
              m_wtimeout = tm;
            }
        }
    }
 
    void initReadPolicy(const coil::Properties& prop)
    {
      std::string policy(prop["read.empty_policy"]);
      if (policy == "readback")
        {
          m_readback = true;
          m_timedread = false;
        }
      else if (policy == "do_nothing")
        {
          m_readback = false;
          m_timedread = false;
        }
      else if (policy == "block")
        {
          m_readback = false;
          m_timedread = true;
          std::chrono::nanoseconds tm;
          if (coil::stringTo(tm, prop["read.timeout"].c_str()))
            {
              m_rtimeout = tm;
            }
        }
    }
 
  private:
    bool m_overwrite{true};
 
    bool m_readback{true};
 
    bool m_timedwrite{false};
    bool m_timedread{false};
 
    std::chrono::nanoseconds m_wtimeout{std::chrono::seconds(1)};
 
    std::chrono::nanoseconds m_rtimeout{std::chrono::seconds(1)};
 
    size_t m_length;
 
    size_t m_wpos{0};
 
    size_t m_rpos{0};
 
    size_t m_fillcount{0};
 
    size_t m_wcount{0};
 
    std::vector<DataType> m_buffer;
 
    struct condition
    {
      condition() {}
      std::condition_variable cond;
      std::mutex mutex;
    };
 
    mutable std::mutex m_posmutex;
 
    condition m_empty;
 
    condition m_full;
  };
 
  template <class T> RingBuffer<T>::~RingBuffer() = default; // no-inline because of its size.
} // namespace RTC
 
#endif  // RTC_RINGBUFFER_H