00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import threading
00019
00020 import OpenRTM
00021 import RTC, RTC__POA
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 class ScopedLock:
00037 def __init__(self, mutex):
00038 self.mutex = mutex
00039 self.mutex.acquire()
00040
00041 def __del__(self):
00042 self.mutex.release()
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 class StateHolder:
00062 def __init__(self):
00063 self.curr = None
00064 self.prev = None
00065 self.next = None
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156 class StateMachine:
00157 state_array = (RTC.INACTIVE_STATE,
00158 RTC.ACTIVE_STATE,
00159 RTC.ERROR_STATE,
00160 RTC.UNKNOWN_STATE)
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176 def __init__(self, num_of_state):
00177 self._num = num_of_state
00178 self._entry = {}
00179 self._predo = {}
00180 self._do = {}
00181 self._postdo = {}
00182 self._exit = {}
00183
00184 self.setNullFunc(self._entry, None)
00185 self.setNullFunc(self._do, None)
00186 self.setNullFunc(self._exit, None)
00187 self.setNullFunc(self._predo, None)
00188 self.setNullFunc(self._postdo, None)
00189 self._transit = None
00190 self._mutex = threading.RLock()
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205 def setNOP(self, call_back):
00206 self.setNullFunc(self._entry, call_back)
00207 self.setNullFunc(self._do, call_back)
00208 self.setNullFunc(self._exit, call_back)
00209 self.setNullFunc(self._predo, call_back)
00210 self.setNullFunc(self._postdo, call_back)
00211 self._transit = call_back
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226 def setListener(self, listener):
00227 self._listener = listener
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245 def setEntryAction(self, state, call_back):
00246 if self._entry.has_key(state):
00247 self._entry[state] = call_back
00248 else:
00249 self._entry.setdefault(state, call_back)
00250 return True
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268 def setPreDoAction(self, state, call_back):
00269 if self._predo.has_key(state):
00270 self._predo[state] = call_back
00271 else:
00272 self._predo.setdefault(state, call_back)
00273 return True
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291 def setDoAction(self, state, call_back):
00292 if self._do.has_key(state):
00293 self._do[state] = call_back
00294 else:
00295 self._do.setdefault(state, call_back)
00296 return True
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314 def setPostDoAction(self, state, call_back):
00315 if self._postdo.has_key(state):
00316 self._postdo[state] = call_back
00317 else:
00318 self._postdo.setdefault(state, call_back)
00319 return True
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337 def setExitAction(self, state, call_back):
00338 if self._exit.has_key(state):
00339 self._exit[state] = call_back
00340 else:
00341 self._exit.setdefault(state, call_back)
00342 return True
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360 def setTransitionAction(self, call_back):
00361 self._transit = call_back
00362 return True
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377 def setStartState(self, states):
00378 self._states = StateHolder()
00379 self._states.curr = states.curr
00380 self._states.prev = states.prev
00381 self._states.next = states.next
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398 def getStates(self):
00399 guard = ScopedLock(self._mutex)
00400 return self._states
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416 def getState(self):
00417 guard = ScopedLock(self._mutex)
00418 return self._states.curr
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435 def isIn(self, state):
00436 guard = ScopedLock(self._mutex)
00437 if self._states.curr == state:
00438 return True
00439 else:
00440 return False
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459 def goTo(self, state):
00460 guard = ScopedLock(self._mutex)
00461 self._states.next = state
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476 def worker(self):
00477 states = StateHolder()
00478 self.sync(states)
00479
00480
00481 if states.curr == states.next:
00482
00483 if self._predo[states.curr]:
00484 self._predo[states.curr](states)
00485 if self.need_trans():
00486 return
00487
00488
00489 if self._do[states.curr]:
00490 self._do[states.curr](states)
00491 if self.need_trans():
00492 return
00493
00494
00495 if self._postdo[states.curr]:
00496 self._postdo[states.curr](states)
00497
00498 else:
00499 if self._exit[states.curr]:
00500 self._exit[states.curr](states)
00501 self.sync(states)
00502
00503
00504 if states.curr != states.next:
00505 states.curr = states.next
00506 if self._entry[states.curr]:
00507 self._entry[states.curr](states)
00508 self.update_curr(states.curr)
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524 def setNullFunc(self, s, nullfunc):
00525 for i in range(self._num):
00526 if s.has_key(StateMachine.state_array[i]):
00527 s[StateMachine.state_array[i]] = nullfunc
00528 else:
00529 s.setdefault(StateMachine.state_array[i], nullfunc)
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541 def sync(self, states):
00542 guard = ScopedLock(self._mutex)
00543 states.prev = self._states.prev
00544 states.curr = self._states.curr
00545 states.next = self._states.next
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559 def need_trans(self):
00560 guard = ScopedLock(self._mutex)
00561 return (self._states.curr != self._states.next)
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573 def update_curr(self, curr):
00574 guard = ScopedLock(self._mutex)
00575 self._states.curr = curr