openrtm_aist_python: StateMachine.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: euc-jp -*-
00003 
00004 ##
00005 # @file StateMachine.py
00006 # @brief State machine template class
00007 # @date $Date: 2007/08/30$
00008 # @author Noriaki Ando <n-ando@aist.go.jp> and Shinji Kurihara
00009 #
00010 # Copyright (C) 2006-2008
00011 #     Task-intelligence Research Group,
00012 #     Intelligent Systems Research Institute,
00013 #     National Institute of
00014 #         Advanced Industrial Science and Technology (AIST), Japan
00015 #     All rights reserved.
00016 
00017 
00018 import threading
00019 
00020 import OpenRTM_aist
00021 import RTC
00022 
00023 
00024 ##
00025 # @if jp
00026 # @class StateHolder
00027 # @brief 状態保持用クラス
00028 # 
00029 # 状態を保持するためのホルダークラス。
00030 # 現在の状態と、１つ前の状態、遷移予定の状態を保持する。
00031 #
00032 # @param State 保持する状態の型
00033 #
00034 # @since 0.4.0
00035 #
00036 # @else
00037 #
00038 # @endif
00039 class StateHolder:
00040   def __init__(self):
00041     self.curr = None
00042     self.prev = None
00043     self.next = None
00044 
00045 
00046 ##
00047 # @if jp
00048 #
00049 # @class StateMachine
00050 #
00051 # @brief 状態マシンクラス
00052 #
00053 # StateMachine クラスは状態マシンを実現するクラスである。
00054 #
00055 # 例: ActiveObjectは状態マシンを持つアクティブオブジェクトであるとする。
00056 # 状態は3状態 INACTIVE, ACTIVE, ERROR があり、各状態でのEntryやExit動作を
00057 # 定義したいとすると、以下のように実現される。
00058 # <pre>
00059 # class ActiveObject:
00060 #   class MyState:
00061 #     INACTIVE, ACTIVE, ERROR = range(3)
00062 # 
00063 #   def __init__(self):
00064 #     m_sm = StateMachine(3)
00065 #     m_sm.setNOP(nullAction)
00066 #     m_sm.setListener(self)
00067 # 
00068 #     m_sm.setExitAction(MyState.INACTIVE, self.inactiveExit)
00069 #       : 
00070 #     m_sm.setPostDoAction(MyState.ERROR, self.errorPostDo)
00071 #     m_sm.setTransitionAction(self.transition); 
00072 # 
00073 #   def nullAction(myStates):
00074 #     pass
00075 #   def inactiveExit(myStates):
00076 #     pass
00077 #     : 
00078 #   def errorPostDo(myStates):
00079 #     pass
00080 #   def transition(myStates:
00081 #     pass
00082 # </pre>
00083 # 状態を持たせたいクラスは以下の条件を満たすように実装しなければならない。
00084 # <ol>
00085 # <li> 内部クラスで状態を定義
00086 # <li> StateMachine のコンストラクタ引数は状態の数
00087 # <li> 以下のアクション関数を(Return _function_name_(States)) の関数として設定
00088 # <ol>
00089 #  <li> 何もしない関数を必ず定義し、setNOP で与えなければならない
00090 #  <li> 各状態毎に, set(Entry|PreDo|Do|PostDo|Exit)Action でアクションを設定
00091 #  <li> 状態遷移時のアクションを setTransitionAction() で設定。
00092 # </ol>
00093 # <li> 遷移時のアクションは、与えられた現在状態、次状態、前状態を元に、
00094 #   ユーザが実装しなければならない。
00095 # <li> 状態の変更は goTo() で、状態のチェックは isIn(state) で行う。
00096 # <li> goTo()は次状態を強制的にセットする関数であり、遷移の可否は、
00097 #   ユーザが現在状態を取得し判断するロジックを実装しなければならない。
00098 # </ol>
00099 #
00100 # このクラスは、一つの状態に対して、
00101 # <ul>
00102 # <li> Entry action
00103 # <li> PreDo action
00104 # <li> Do action
00105 # <li> PostDo action
00106 # <li> Exit action
00107 # </ul>
00108 # 5つのアクションが定義することができる。
00109 # Transition action はあらゆる状態間遷移で呼び出されるアクションで、
00110 # その振る舞いはユーザが定義しなければならない。
00111 # 
00112 # このクラスは以下のようなタイミングで各アクションが実行される。
00113 #
00114 # <ul>
00115 # <li> 状態が変更され(A->B)状態が遷移する場合 <br>
00116 # (A:Exit)->|(状態更新:A->B)->(B:Entry)->(B:PreDo)->(B:Do)->(B:PostDo)
00117 #
00118 # <li> 状態が変更されず、B状態を維持する場合 (|はステップの区切りを表す)<br>
00119 # (B(n-1):PostDo)->|(B(n):PreDo)->(B(n):Do)->(B(n):PostDo)->|(B(n+1):PreDo)<br>
00120 # PreDo, Do, PostDo が繰り返し実行される。
00121 #
00122 # <li> 自己遷移する場合 <br>
00123 # (B(n-1):PostDo)->(B(n-1):Exit)->|(B(n):Entry)->(B(n):PreDo) <br>
00124 # 一旦 Exit が呼ばれた後、Entry が実行され、以降は前項と同じ動作をする。
00125 # </ul>
00126 #
00127 # @since 0.4.0
00128 #
00129 # @else
00130 #
00131 # @brief
00132 #
00133 # @endif
00134 class StateMachine:
00135   """
00136   """
00137 
00138   state_array = (RTC.CREATED_STATE,
00139                  RTC.INACTIVE_STATE,
00140                  RTC.ACTIVE_STATE,
00141                  RTC.ERROR_STATE)
00142 
00143   ##
00144   # @if jp
00145   # @brief コンストラクタ
00146   #
00147   # コンストラクタ
00148   #
00149   # @param self
00150   # @param num_of_state ステートマシン中の状態数
00151   #
00152   # @else
00153   # @brief Constructor
00154   # @endif
00155   def __init__(self, num_of_state):
00156     self._num = num_of_state
00157     self._entry  = {}
00158     self._predo  = {}
00159     self._do     = {}
00160     self._postdo = {}
00161     self._exit   = {}
00162 
00163     self.setNullFunc(self._entry,  None)
00164     self.setNullFunc(self._do,     None)
00165     self.setNullFunc(self._exit,   None)
00166     self.setNullFunc(self._predo,  None)
00167     self.setNullFunc(self._postdo, None)
00168     self._transit = None
00169     self._mutex = threading.RLock()
00170 
00171 
00172   ##
00173   # @if jp
00174   # @brief NOP関数を登録する
00175   #
00176   # NOP関数(何もしない関数)を登録する。
00177   #
00178   # @param self
00179   # @param call_back コールバック関数
00180   #
00181   # @else
00182   # @brief Set NOP function
00183   # @endif
00184   def setNOP(self, call_back):
00185     self.setNullFunc(self._entry,  call_back)
00186     self.setNullFunc(self._do,     call_back)
00187     self.setNullFunc(self._exit,   call_back)
00188     self.setNullFunc(self._predo,  call_back)
00189     self.setNullFunc(self._postdo, call_back)
00190     self._transit = call_back
00191 
00192 
00193   ##
00194   # @if jp
00195   # @brief Listener オブジェクトを登録する
00196   #
00197   # 各種アクション実行時に呼び出される Listener オブジェクトを登録する。
00198   #
00199   # @param self
00200   # @param listener Listener オブジェクト
00201   #
00202   # @else
00203   # @brief Set Listener Object
00204   # @endif
00205   def setListener(self, listener):
00206     self._listener = listener
00207 
00208 
00209   ##
00210   # @if jp
00211   # @brief Entry action 関数を登録する
00212   #
00213   # 各状態に入った際に実行される Entry action 用コールバック関数を登録する。
00214   #
00215   # @param self
00216   # @param state 登録対象状態
00217   # @param call_back Entry action 用コールバック関数
00218   #
00219   # @return アクション実行結果
00220   #
00221   # @else
00222   # @brief Set Entry action function
00223   # @endif
00224   def setEntryAction(self, state, call_back):
00225     if self._entry.has_key(state):
00226       self._entry[state] = call_back
00227     else:
00228       self._entry.setdefault(state, call_back)
00229     return True
00230 
00231 
00232   ##
00233   # @if jp
00234   # @brief PreDo action 関数を登録する
00235   #
00236   # 各状態内で実行される PreDo action 用コールバック関数を登録する。
00237   #
00238   # @param self
00239   # @param state 登録対象状態
00240   # @param call_back PreDo action 用コールバック関数
00241   #
00242   # @return アクション実行結果
00243   #
00244   # @else
00245   # @brief Set PreDo action function
00246   # @endif
00247   def setPreDoAction(self, state, call_back):
00248     if self._predo.has_key(state):
00249       self._predo[state] = call_back
00250     else:
00251       self._predo.setdefault(state, call_back)
00252     return True
00253 
00254 
00255   ##
00256   # @if jp
00257   # @brief Do action 関数を登録する
00258   #
00259   # 各状態内で実行される Do action 用コールバック関数を登録する。
00260   #
00261   # @param self
00262   # @param state 登録対象状態
00263   # @param call_back Do action 用コールバック関数
00264   #
00265   # @return アクション実行結果
00266   #
00267   # @else
00268   # @brief Set Do action function
00269   # @endif
00270   def setDoAction(self, state, call_back):
00271     if self._do.has_key(state):
00272       self._do[state] = call_back
00273     else:
00274       self._do.setdefault(state, call_back)
00275     return True
00276 
00277 
00278   ##
00279   # @if jp
00280   # @brief PostDo action 関数を登録する
00281   #
00282   # 各状態内で実行される PostDo action 用コールバック関数を登録する。
00283   #
00284   # @param self
00285   # @param state 登録対象状態
00286   # @param call_back PostDo action 用コールバック関数
00287   #
00288   # @return アクション実行結果
00289   #
00290   # @else
00291   # @brief Set PostDo action function
00292   # @endif
00293   def setPostDoAction(self, state, call_back):
00294     if self._postdo.has_key(state):
00295       self._postdo[state] = call_back
00296     else:
00297       self._postdo.setdefault(state, call_back)
00298     return True
00299 
00300 
00301   ##
00302   # @if jp
00303   # @brief Exit action 関数を登録する
00304   #
00305   # 各状態内で実行される Exit action 用コールバック関数を登録する。
00306   #
00307   # @param self
00308   # @param state 登録対象状態
00309   # @param call_back Exit action 用コールバック関数
00310   #
00311   # @return アクション実行結果
00312   #
00313   # @else
00314   # @brief Set Exit action function
00315   # @endif
00316   def setExitAction(self, state, call_back):
00317     if self._exit.has_key(state):
00318       self._exit[state] = call_back
00319     else:
00320       self._exit.setdefault(state, call_back)
00321     return True
00322 
00323 
00324   ##
00325   # @if jp
00326   # @brief State transition action 関数を登録する
00327   #
00328   # 状態遷移時に実行される State transition action 用コールバック関数を
00329   # 登録する。
00330   #
00331   # @param self
00332   # @param call_back State transition 用コールバック関数
00333   #
00334   # @return アクション実行結果
00335   #
00336   # @else
00337   # @brief Set state transition action function
00338   # @endif
00339   def setTransitionAction(self, call_back):
00340     self._transit = call_back
00341     return True
00342 
00343 
00344   ##
00345   # @if jp
00346   # @brief 初期状態をセットする
00347   #
00348   # ステートマシンの初期状態を設定する。
00349   #
00350   # @param self
00351   # @param states 初期状態
00352   #
00353   # @else
00354   # @brief Set Exit action function
00355   # @endif
00356   def setStartState(self, states):
00357     self._states = StateHolder()
00358     self._states.curr = states.curr
00359     self._states.prev = states.prev
00360     self._states.next = states.next
00361 
00362 
00363   ##
00364   # @if jp
00365   # @brief 状態を取得する
00366   #
00367   # 状態情報を取得する。
00368   # 現在の状態、１つ前の状態、遷移予定の状態を取得することができる。
00369   #
00370   # @param self
00371   #
00372   # @return 状態情報
00373   #
00374   # @else
00375   # @brief Get state machine's status
00376   # @endif
00377   def getStates(self):
00378     guard = OpenRTM_aist.ScopedLock(self._mutex)
00379     return self._states
00380 
00381 
00382   ##
00383   # @if jp
00384   # @brief 現在の状態を取得する
00385   #
00386   # 現在の状態を取得する。
00387   #
00388   # @param self
00389   #
00390   # @return 現在の状態
00391   #
00392   # @else
00393   # @brief Get current state
00394   # @endif
00395   def getState(self):
00396     guard = OpenRTM_aist.ScopedLock(self._mutex)
00397     return self._states.curr
00398 
00399 
00400   ##
00401   # @if jp
00402   # @brief 現在状態を確認
00403   #
00404   # 現在の状態が、引数で指定した状態と一致するか確認する。
00405   #
00406   # @param self
00407   # @param state 確認対象状態
00408   #
00409   # @return 状態確認結果
00410   #
00411   # @else
00412   # @brief Evaluate current status
00413   # @endif
00414   def isIn(self, state):
00415     guard = OpenRTM_aist.ScopedLock(self._mutex)
00416     if self._states.curr == state:
00417       return True
00418     else:
00419       return False
00420 
00421 
00422   ##
00423   # @if jp
00424   # @brief 状態を遷移
00425   #
00426   # 指定した状態に状態を遷移する。
00427   # 本関数は次状態を強制的にセットする関数である。
00428   # このため、遷移の可否は、ユーザが現在状態を取得し判断するロジックを
00429   # 実装しなければならない。
00430   # 遷移先が現在の状態と同じ場合には、自己遷移フラグをセットする。
00431   #
00432   # @param self
00433   # @param state 遷移先状態
00434   #
00435   # @else
00436   # @brief Change status
00437   # @endif
00438   def goTo(self, state):
00439     guard = OpenRTM_aist.ScopedLock(self._mutex)
00440     self._states.next = state
00441 
00442 
00443   ##
00444   # @if jp
00445   # @brief 駆動関数
00446   #
00447   # ステートマシンの駆動関数。
00448   # 実際の状態遷移および状態遷移発生時の各アクションの呼びだしを実行する。
00449   #
00450   # @param self
00451   #
00452   # @else
00453   # @brief Worker function
00454   # @endif
00455   def worker(self):
00456     states = StateHolder()
00457     self.sync(states)
00458 
00459     # If no state transition required, execute set of do-actions
00460     if states.curr == states.next:
00461       # pre-do
00462       if self._predo[states.curr]:
00463         self._predo[states.curr](states)
00464       if self.need_trans():
00465         return
00466 
00467       # do
00468       if self._do[states.curr]:
00469         self._do[states.curr](states)
00470       if self.need_trans():
00471         return
00472 
00473       # post-do
00474       if self._postdo[states.curr]:
00475         self._postdo[states.curr](states)
00476     # If state transition required, exit current state and enter next state
00477     else:
00478       if self._exit[states.curr]:
00479         self._exit[states.curr](states)
00480       self.sync(states)
00481 
00482       # If state transition still required, move to the next state
00483       if states.curr != states.next:
00484         states.curr = states.next
00485         if self._entry[states.curr]:
00486           self._entry[states.curr](states)
00487         self.update_curr(states.curr)
00488 
00489 
00490   ##
00491   # @if jp
00492   # @brief NOP関数を設定
00493   #
00494   # NOP関数(何もしない関数)を登録する。
00495   #
00496   # @param self
00497   # @param s コールバック関数設定先
00498   # @param nullfunc コールバック関数(NOP関数)
00499   #
00500   # @else
00501   # @brief Worker function
00502   # @endif
00503   def setNullFunc(self, s, nullfunc):
00504     for i in range(self._num):
00505       if s.has_key(StateMachine.state_array[i]):
00506         s[StateMachine.state_array[i]] = nullfunc
00507       else:
00508         s.setdefault(StateMachine.state_array[i], nullfunc)
00509 
00510 
00511   ##
00512   # @if jp
00513   # @brief 状態の同期処理
00514   #
00515   # @param self
00516   # @param states OpenRTM_aist.StateHolder<RTC.LifeCycleState>
00517   #
00518   # @else
00519   # @endif
00520   def sync(self, states):
00521     guard = OpenRTM_aist.ScopedLock(self._mutex)
00522     states.prev = self._states.prev
00523     states.curr = self._states.curr
00524     states.next = self._states.next
00525     
00526 
00527 
00528   ##
00529   # @if jp
00530   # @brief 遷移の必要性チェック
00531   #
00532   # @param self
00533   #
00534   # @return 遷移必要性確認結果
00535   #
00536   # @else
00537   # @endif
00538   def need_trans(self):
00539     guard = OpenRTM_aist.ScopedLock(self._mutex)
00540     return (self._states.curr != self._states.next)
00541 
00542 
00543   ##
00544   # @if jp
00545   # @brief 現在状態の更新
00546   #
00547   # @param self
00548   # @param curr RTC.LifeCycleState
00549   #
00550   # @else
00551   # @endif
00552   def update_curr(self, curr):
00553     guard = OpenRTM_aist.ScopedLock(self._mutex)
00554     self._states.curr = curr