StateMachine.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: euc-jp -*-
 
 
 
 
import threading
 
import OpenRTM_aist
import RTC
 
 
 
class StateHolder:
  def __init__(self):
    self.curr = None
    self.prev = None
    self.next = None
 
 
 
class StateMachine:
  """
  """
 
  state_array = (RTC.CREATED_STATE,
                 RTC.INACTIVE_STATE,
                 RTC.ACTIVE_STATE,
                 RTC.ERROR_STATE)
 
  
  def __init__(self, num_of_state):
    self._num = num_of_state
    self._entry  = {}
    self._predo  = {}
    self._do     = {}
    self._postdo = {}
    self._exit   = {}
 
    self.setNullFunc(self._entry,  None)
    self.setNullFunc(self._do,     None)
    self.setNullFunc(self._exit,   None)
    self.setNullFunc(self._predo,  None)
    self.setNullFunc(self._postdo, None)
    self._transit = None
    self._mutex = threading.RLock()
 
 
  
  def setNOP(self, call_back):
    self.setNullFunc(self._entry,  call_back)
    self.setNullFunc(self._do,     call_back)
    self.setNullFunc(self._exit,   call_back)
    self.setNullFunc(self._predo,  call_back)
    self.setNullFunc(self._postdo, call_back)
    self._transit = call_back
 
 
  
  def setListener(self, listener):
    self._listener = listener
 
 
  
  def setEntryAction(self, state, call_back):
    if state in self._entry:
      self._entry[state] = call_back
    else:
      self._entry.setdefault(state, call_back)
    return True
 
 
  
  def setPreDoAction(self, state, call_back):
    if state in self._predo:
      self._predo[state] = call_back
    else:
      self._predo.setdefault(state, call_back)
    return True
 
 
  
  def setDoAction(self, state, call_back):
    if state in self._do:
      self._do[state] = call_back
    else:
      self._do.setdefault(state, call_back)
    return True
 
 
  
  def setPostDoAction(self, state, call_back):
    if state in self._postdo:
      self._postdo[state] = call_back
    else:
      self._postdo.setdefault(state, call_back)
    return True
 
 
  
  def setExitAction(self, state, call_back):
    if state in self._exit:
      self._exit[state] = call_back
    else:
      self._exit.setdefault(state, call_back)
    return True
 
 
  
  def setTransitionAction(self, call_back):
    self._transit = call_back
    return True
 
 
  
  def setStartState(self, states):
    self._states = StateHolder()
    self._states.curr = states.curr
    self._states.prev = states.prev
    self._states.next = states.next
 
 
  
  def getStates(self):
    guard = OpenRTM_aist.ScopedLock(self._mutex)
    return self._states
 
 
  
  def getState(self):
    guard = OpenRTM_aist.ScopedLock(self._mutex)
    return self._states.curr
 
 
  
  def isIn(self, state):
    guard = OpenRTM_aist.ScopedLock(self._mutex)
    if self._states.curr == state:
      return True
    else:
      return False
 
 
  
  def goTo(self, state):
    guard = OpenRTM_aist.ScopedLock(self._mutex)
    self._states.next = state
 
 
  
  def worker(self):
    states = StateHolder()
    self.sync(states)
 
    # If no state transition required, execute set of do-actions
    if states.curr == states.next:
      # pre-do
      if self._predo[states.curr]:
        self._predo[states.curr](states)
      if self.need_trans():
        return
 
      # do
      if self._do[states.curr]:
        self._do[states.curr](states)
      if self.need_trans():
        return
 
      # post-do
      if self._postdo[states.curr]:
        self._postdo[states.curr](states)
    # If state transition required, exit current state and enter next state
    else:
      if self._exit[states.curr]:
        self._exit[states.curr](states)
      self.sync(states)
 
      # If state transition still required, move to the next state
      if states.curr != states.next:
        states.curr = states.next
        if self._entry[states.curr]:
          self._entry[states.curr](states)
        self.update_curr(states.curr)
 
 
  
  def setNullFunc(self, s, nullfunc):
    for i in range(self._num):
      if StateMachine.state_array[i] in s:
        s[StateMachine.state_array[i]] = nullfunc
      else:
        s.setdefault(StateMachine.state_array[i], nullfunc)
 
 
  
  def sync(self, states):
    guard = OpenRTM_aist.ScopedLock(self._mutex)
    states.prev = self._states.prev
    states.curr = self._states.curr
    states.next = self._states.next
    
 
 
  
  def need_trans(self):
    guard = OpenRTM_aist.ScopedLock(self._mutex)
    return (self._states.curr != self._states.next)
 
 
  
  def update_curr(self, curr):
    guard = OpenRTM_aist.ScopedLock(self._mutex)
    self._states.curr = curr