RingBuffer.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: euc-jp -*-
 
 
 
import threading
import OpenRTM_aist
 
 
 
class RingBuffer(OpenRTM_aist.BufferBase):
  """
  """
 
  RINGBUFFER_DEFAULT_LENGTH = 8
 
  
  def __init__(self, length=RINGBUFFER_DEFAULT_LENGTH):
    self._overwrite = True
    self._readback = True
    self._timedwrite = False
    self._timedread  = False
    self._wtimeout = OpenRTM_aist.TimeValue(1,0)
    self._rtimeout = OpenRTM_aist.TimeValue(1,0)
    self._length   = length
    self._wpos = 0
    self._rpos = 0
    self._fillcount = 0
    self._wcount = 0
    self._buffer = [None for i in range(self._length)]
    self._pos_mutex = threading.RLock()
    self._full_mutex = threading.RLock()
    self._empty_mutex = threading.RLock()
    self._full_cond = threading.Condition(self._full_mutex)
    self._empty_cond = threading.Condition(self._empty_mutex)
 
 
    self.reset()
 
 
  
  def init(self, prop):
    self.__initLength(prop)
    self.__initWritePolicy(prop)
    self.__initReadPolicy(prop)
 
 
  
  def length(self, n = None):
    if n is None:
      guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
      return self._length
 
    if n < 1:
      return OpenRTM_aist.BufferStatus.NOT_SUPPORTED
 
    self._buffer = [None for i in range(n)]
    self._length = n
    self.reset()
    return OpenRTM_aist.BufferStatus.BUFFER_OK
 
  
  
  def reset(self):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    self._fillcount = 0
    self._wcount = 0
    self._wpos = 0
    self._rpos = 0
    return OpenRTM_aist.BufferStatus.BUFFER_OK
 
 
  
  def wptr(self, n = 0):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    return self._buffer[(self._wpos + n + self._length) % self._length]
 
    
  
  def advanceWptr(self, 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 = OpenRTM_aist.ScopedLock(self._pos_mutex)
    if (n > 0 and n > (self._length - self._fillcount)) or \
          (n < 0 and n < (-self._fillcount)):
      return OpenRTM_aist.BufferStatus.PRECONDITION_NOT_MET
 
    self._wpos = (self._wpos + n + self._length) % self._length
    self._fillcount += n
    return OpenRTM_aist.BufferStatus.BUFFER_OK
 
 
  
 
  #
  # @param value Target data to write.
  #
  # @return BUFFER_OK:    Successful
  #         BUFFER_ERROR: Failed
  #
  # @endif
  #
  # ReturnCode put(const DataType& value)
  def put(self, value):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    self._buffer[self._wpos] = value
    return OpenRTM_aist.BufferStatus.BUFFER_OK
    
  
  def write(self, value, sec = -1, nsec = 0):
    try:
      self._full_cond.acquire()
      if self.full():
        timedwrite = self._timedwrite # default is False
        overwrite  = self._overwrite  # default is True
 
        if not (sec < 0): # if second arg is set -> block mode
          timedwrite = True
          overwrite  = False
 
        if overwrite and not timedwrite:       # "overwrite" mode
          self.advanceRptr()
 
        elif not overwrite and not timedwrite: # "do_nothiong" mode
          self._full_cond.release()
          return OpenRTM_aist.BufferStatus.BUFFER_FULL
 
        elif not overwrite and timedwrite:     # "block" mode
 
          if sec < 0:
            sec = self._wtimeout.sec()
            nsec = self._wtimeout.usec() * 1000
 
          # true: signaled, false: timeout
          if not self._full_cond.wait(sec + (nsec/1000000000.0)):
            self._full_cond.release()
            return OpenRTM_aist.BufferStatus.TIMEOUT
 
        else: # unknown condition
          self._full_cond.release()
          return OpenRTM_aist.BufferStatus.PRECONDITION_NOT_MET
      
      self._full_cond.release()
 
      self.put(value)
      
      self._empty_cond.acquire()
      empty = self.empty()
      if empty:
        self.advanceWptr(1)
        self._empty_cond.notify()
      else:
        self.advanceWptr(1)
      self._empty_cond.release()
 
      return OpenRTM_aist.BufferStatus.BUFFER_OK
    except:
      return OpenRTM_aist.BufferStatus.BUFFER_OK
 
    
  
  def writable(self):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    return self._length - self._fillcount
    
 
  
  def full(self):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    return self._length == self._fillcount
    
 
  
  def rptr(self, n = 0):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    return self._buffer[(self._rpos + n + self._length) % self._length]
 
    
  
  def advanceRptr(self, 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 = OpenRTM_aist.ScopedLock(self._pos_mutex)
    if (n > 0 and n > self._fillcount) or \
          (n < 0 and n < (self._fillcount - self._length)):
      return OpenRTM_aist.BufferStatus.PRECONDITION_NOT_MET
 
    self._rpos = (self._rpos + n + self._length) % self._length
    self._fillcount -= n
    return OpenRTM_aist.BufferStatus.BUFFER_OK
 
 
    
  
  def get(self, value=None):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    if value is None:
      return self._buffer[self._rpos]
 
    value[0] = self._buffer[self._rpos]
    return OpenRTM_aist.BufferStatus.BUFFER_OK
    
    
  
  def read(self, value, sec = -1, nsec = 0):
    self._empty_cond.acquire()
      
    if self.empty():
      timedread = self._timedread
      readback  = self._readback
 
      if not (sec < 0):  # if second arg is set -> block mode
        timedread = True
        readback  = False
        sec = self._rtimeout.sec()
        nsec = self._rtimeout.usec() * 1000
 
      if readback and  not timedread:      # "readback" mode
        if not self._wcount > 0:
          self._empty_cond.release()
          return OpenRTM_aist.BufferStatus.BUFFER_EMPTY
        self.advanceRptr(-1)
 
      elif not readback and not timedread: # "do_nothiong" mode
        self._empty_cond.release()
        return OpenRTM_aist.BufferStatus.BUFFER_EMPTY
 
      elif not readback and timedread:     # "block" mode
        if sec < 0:
          sec = self._rtimeout.sec()
          nsec = self._rtimeout.usec() * 1000
        #  true: signaled, false: timeout
        if not self._empty_cond.wait(sec + (nsec/1000000000.0)):
          self._empty_cond.release()
          return OpenRTM_aist.BufferStatus.TIMEOUT
 
      else:                              # unknown condition
        self._empty_cond.release()
        return OpenRTM_aist.BufferStatus.PRECONDITION_NOT_MET
      
    self._empty_cond.release()
 
    val = self.get()
 
    if len(value) > 0:
      value[0] = val
    else:
      value.append(val)
 
    self._full_cond.acquire()
    full_ = self.full()
 
    if full_:
      self.advanceRptr()
      self._full_cond.notify()
    else:
      self.advanceRptr()
 
    self._full_cond.release()
 
 
    return OpenRTM_aist.BufferStatus.BUFFER_OK
 
    
  
  def readable(self):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    return self._fillcount
    
 
  
  def empty(self):
    guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
    return self._fillcount == 0
 
    
  
  def __initLength(self, prop):
    if prop.getProperty("length"):
      n = [0]
      if OpenRTM_aist.stringTo(n, prop.getProperty("length")):
        n = n[0]
        if n > 0:
          self.length(n)
 
    
  
  def __initWritePolicy(self, prop):
    policy = OpenRTM_aist.normalize([prop.getProperty("write.full_policy")])
 
    if policy == "overwrite":
      self._overwrite  = True
      self._timedwrite = False
    
    elif policy == "do_nothing":
      self._overwrite  = False
      self._timedwrite = False
 
    elif policy == "block":
      self._overwrite  = False
      self._timedwrite = True
 
      tm = [0.0]
      if OpenRTM_aist.stringTo(tm, prop.getProperty("write.timeout")):
        tm = tm[0]
        if not (tm < 0):
          self._wtimeout.set_time(tm)
 
    
  
  def __initReadPolicy(self, prop):
    policy = prop.getProperty("read.empty_policy")
 
    if policy == "readback":
      self._readback  = True
      self._timedread = False
 
    elif policy == "do_nothing":
      self._readback  = False
      self._timedread = False
 
    elif policy == "block":
      self._readback  = False
      self._timedread = True
      tm = [0.0]
      if OpenRTM_aist.stringTo(tm, prop.getProperty("read.timeout")):
        self._rtimeout.set_time(tm[0])