RingBuffer.h

説明を見る。

// -*- C++ -*-
#ifndef RTC_RINGBUFFER_H
#define RTC_RINGBUFFER_H

#include <vector>
#include <algorithm>
#include <iostream>

#include <coil/TimeValue.h>
#include <coil/Mutex.h>
#include <coil/Guard.h>
#include <coil/Condition.h>
#include <coil/stringutil.h>

#include <rtm/BufferBase.h>
#include <rtm/BufferStatus.h>

#define RINGBUFFER_DEFAULT_LENGTH 8

namespace RTC
{
  template <class DataType>
  class RingBuffer
    : public BufferBase<DataType>
  {
  public:
    BUFFERSTATUS_ENUM
    typedef coil::Guard<coil::Mutex> Guard;
    RingBuffer(long int length = RINGBUFFER_DEFAULT_LENGTH)
      : m_overwrite(true), m_readback(true),
        m_timedwrite(false), m_timedread(false),
        m_wtimeout(1, 0), m_rtimeout(1, 0),
        m_length(length),
        m_wpos(0), m_rpos(0), m_fillcount(0), m_wcount(0),
        m_buffer(m_length)
    {
      this->reset();
    }
    
    virtual ~RingBuffer(void)
    {
    }
    
    virtual void init(const coil::Properties& prop)
    {
      initLength(prop);
      initWritePolicy(prop);
      initReadPolicy(prop);
    }
    
    virtual size_t length(void) const
    {
      Guard guard(m_posmutex);
      return m_length;
    }
    
    virtual ReturnCode length(size_t n)
    {
      m_buffer.resize(n);
      m_length = n;
      this->reset();
      return ::RTC::BufferStatus::BUFFER_OK; //BUFFER_OK;
    }
    
    virtual ReturnCode reset()
    {
      Guard guard(m_posmutex);
      m_fillcount = 0;
      m_wcount = 0;
      m_wpos = 0;
      m_rpos = 0;
      return ::RTC::BufferStatus::BUFFER_OK;
    }
    
    
    
    //----------------------------------------------------------------------
    virtual DataType* wptr(long int n = 0) 
    {
      Guard guard(m_posmutex);
      return &m_buffer[(m_wpos + n + m_length) % m_length];
    }
    
    virtual ReturnCode advanceWptr(long int n = 1)
    {
      // 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
      Guard guard(m_posmutex);
      if ((n > 0 && n > static_cast<long int>(m_length - m_fillcount)) ||
          (n < 0 && n < static_cast<long int>(-m_fillcount)))
        {
          return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
        }

      m_wpos = (m_wpos + n + m_length) % m_length;
      m_fillcount += n;
      m_wcount += n;
      return ::RTC::BufferStatus::BUFFER_OK;
    }
    virtual ReturnCode put(const DataType& value)
    {
      Guard guard(m_posmutex);
      m_buffer[m_wpos] = value;
      return ::RTC::BufferStatus::BUFFER_OK;
    }
    
    virtual ReturnCode write(const DataType& value,
                             long int sec = -1, long int nsec = 0)
    {
      {
      Guard guard(m_full.mutex);
        
      if (full())
        {
          
          bool timedwrite(m_timedwrite);
          bool overwrite(m_overwrite);

          if (!(sec < 0)) // if second arg is set -> block mode
            {
              timedwrite = true;
              overwrite  = false;
            }

          if (overwrite && !timedwrite)       // "overwrite" mode
            {
              advanceRptr();
            }
          else if (!overwrite && !timedwrite) // "do_nothing" mode
            {
              return ::RTC::BufferStatus::BUFFER_FULL;
            }
          else if (!overwrite && timedwrite)  // "block" mode
            {
              if (sec < 0)
                {
                  sec = m_wtimeout.sec();
                  nsec = m_wtimeout.usec() * 1000;
                }
              //  true: signaled, false: timeout
              if (!m_full.cond.wait(sec, nsec))
                {
                  return ::RTC::BufferStatus::TIMEOUT;
                }
            }
          else                                    // unknown condition
            {
              return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
            }
        }
      }      
    
      put(value);

          {
                Guard eguard(m_empty.mutex);
                if (empty())
                  {
                        // Guard eguard(m_empty.mutex);
                        advanceWptr(1);
                        m_empty.cond.signal();
                  }
                else
                  {
                        advanceWptr(1);
                  }
          }
      return ::RTC::BufferStatus::BUFFER_OK;
    }
    
    virtual size_t writable() const
    {
      Guard guard(m_posmutex);
      return m_length - m_fillcount;
    }
    
    virtual bool full(void) const
    {
      Guard guard(m_posmutex);
      return m_length == m_fillcount;
    }
    
    //----------------------------------------------------------------------
    virtual DataType* rptr(long int n = 0)
    {
      Guard guard(m_posmutex);
      return &(m_buffer[(m_rpos + n + m_length) % m_length]);
    }
    
    virtual ReturnCode advanceRptr(long int n = 1)
    {
      // 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
      Guard guard(m_posmutex);
      if ((n > 0 && n > static_cast<long int>(m_fillcount)) ||
          (n < 0 && n < static_cast<long int>(m_fillcount - m_length)))
        {
          return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
        }

      m_rpos = (m_rpos + n + m_length) % m_length;
      m_fillcount -= n;
      return ::RTC::BufferStatus::BUFFER_OK;
    }
    
    virtual ReturnCode get(DataType& value)
    {
      Guard gaurd(m_posmutex);
      value = m_buffer[m_rpos];
      return ::RTC::BufferStatus::BUFFER_OK;
    }
    
    
    virtual DataType& get()
    {
      Guard gaurd(m_posmutex);
      return m_buffer[m_rpos];
    }
    
    
    virtual ReturnCode read(DataType& value,
                            long int sec = -1, long int nsec = 0)
    {
      {
      Guard gaurd(m_empty.mutex);
      
      if (empty())
        {
          bool timedread(m_timedread);
          bool readback(m_readback);

          if (!(sec < 0)) // if second arg is set -> block mode
            {
              timedread = true;
              readback  = false;
              sec = m_rtimeout.sec();
              nsec = m_rtimeout.usec() * 1000;
            }

          if (readback && !timedread)       // "readback" mode
            {
              if (!(m_wcount > 0))
                {
                  return ::RTC::BufferStatus::BUFFER_EMPTY;
                }
              advanceRptr(-1);
            }
          else if (!readback && !timedread) // "do_nothing" mode
            {
              return ::RTC::BufferStatus::BUFFER_EMPTY;
            }
          else if (!readback && timedread)  // "block" mode
            {
              if (sec < 0)
                {
                  sec = m_rtimeout.sec();
                  nsec = m_rtimeout.usec() * 1000;
                }
              //  true: signaled, false: timeout
              if (!m_empty.cond.wait(sec, nsec))
                {
                  return ::RTC::BufferStatus::TIMEOUT;
                }
            }
          else                                    // unknown condition
            {
              return ::RTC::BufferStatus::PRECONDITION_NOT_MET;
            }
        }
      }
      
      get(value);

          {
                Guard fguard(m_full.mutex);
                if (full())
                  {
                        // Guard fguard(m_full.mutex);
                        advanceRptr(1);
                        m_full.cond.signal();
                  }
                else
                  {
                        advanceRptr(1);
                  }
          }
      return ::RTC::BufferStatus::BUFFER_OK;
    }
    
    virtual size_t readable() const
    {
      Guard guard(m_posmutex);
      return m_fillcount;
    }
    
    virtual bool empty(void) const
    {
      Guard guard(m_posmutex);
      return m_fillcount == 0;
    }
    
  private:
    inline 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);
                }
            }
        }
    }
    
    inline void initWritePolicy(const coil::Properties& prop)
    {
      std::string policy(prop["write.full_policy"]);
      coil::normalize(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;
          
          double tm;
          if (coil::stringTo(tm, prop["write.timeout"].c_str()))
            {
              if (!(tm < 0))
                {
                  m_wtimeout = tm;
                }
            }
        }
    }
    
    inline 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;
          double tm;
          if (coil::stringTo(tm, prop["read.timeout"].c_str()))
            {
              m_rtimeout = tm;
            }
        }
    }
    
  private:
    bool m_overwrite;

    bool m_readback;

    bool m_timedwrite;
    bool m_timedread;

    coil::TimeValue m_wtimeout;

    coil::TimeValue m_rtimeout;

    size_t m_length;

    size_t m_wpos;

    size_t m_rpos;

    size_t m_fillcount;

    size_t m_wcount;

    std::vector<DataType> m_buffer;
    
    struct condition
    {
      condition() : cond(mutex) {}
      coil::Condition<coil::Mutex> cond;
      coil::Mutex mutex;
    };
    
    mutable coil::Mutex m_posmutex;

    condition m_empty;

    condition m_full;
  };
}; // namespace RTC

#endif // RTC_RINGBUFFER_H

---