1
2 import threading
3 import traceback
4 from contextlib import contextmanager
5
6 import smach
7
8 __all__ = ['StateMachine']
12 """StateMachine
13
14 This is a finite state machine smach container. Note that though this is
15 a state machine, it also implements the L{smach.State}
16 interface, so these can be composed hierarchically, if such a pattern is
17 desired.
18
19 States are added to the state machine as 3-tuple specifications:
20 - label
21 - state instance
22 - transitions
23
24 The label is a string, the state instance is any class that implements the
25 L{smach.State} interface, and transitions is a dictionary mapping strings onto
26 strings which represent the transitions out of this new state. Transitions
27 can take one of three forms:
28 - OUTCOME -> STATE_LABEL
29 - OUTCOME -> None (or unspecified)
30 - OUTCOME -> SM_OUTCOME
31 """
32 - def __init__(self, outcomes, input_keys=[], output_keys=[]):
33 """Constructor for smach StateMachine Container.
34
35 @type outcomes: list of strings
36 @param outcomes: The potential outcomes of this state machine.
37 """
38
39
40 smach.container.Container.__init__(self, outcomes, input_keys, output_keys)
41
42
43 self._state_transitioning_lock = threading.Lock()
44
45
46 self._is_running = False
47
48 self._initial_state_label = None
49
50 self._current_label = None
51 self._current_state = None
52 self._current_transitions = None
53 self._current_outcome = None
54
55
56 self._preempted_label = None
57 self._preempted_state = None
58
59
60
61
62 self._states = {}
63 self._transitions = {}
64 self._remappings = {}
65
66
67 self._last_added_label = None
68 self._connector_outcomes = []
69
70
71 self._execute_thread = None
72 self.userdata = smach.UserData()
73
74
75 @staticmethod
76 - def add(label, state, transitions=None, remapping=None):
77 """Add a state to the opened state machine.
78
79 @type label: string
80 @param label: The label of the state being added.
81
82 @param state: An instance of a class implementing the L{State} interface.
83
84 @param transitions: A dictionary mapping state outcomes to other state
85 labels or container outcomes.
86
87 @param remapping: A dictrionary mapping local userdata keys to userdata
88 keys in the container.
89 """
90
91 self = StateMachine._currently_opened_container()
92
93 smach.logdebug('Adding state (%s, %s, %s)' % (label, str(state), str(transitions)))
94
95
96 if self._initial_state_label is None:
97 self._initial_state_label = label
98
99 if transitions is None:
100 transitions = {}
101
102 if remapping is None:
103 remapping = {}
104
105
106 """
107 if 'transitions' in smach.Container._context_kwargs:
108 for outcome, target in smach.Container._context_kwargs['transitions'].iteritems():
109 if outcome not in transitions:
110 transitions[outcome] = target
111 """
112
113
114 self.check_state_spec(label, state, transitions)
115
116
117 if label in self._states:
118 raise smach.InvalidStateError(
119 'Attempting to add state with label "'+label+'" to state machine, but this label is already being used.')
120
121
122 smach.logdebug("Adding state '"+str(label)+"' to the state machine.")
123
124
125 registered_outcomes = state.get_registered_outcomes()
126
127
128 missing_transitions = {o: None for o in registered_outcomes if o not in transitions}
129 transitions.update(missing_transitions)
130 smach.logdebug("State '%s' is missing transitions: %s" % (label, str(missing_transitions)))
131
132
133 self._states[label] = state
134 self._transitions[label] = transitions
135 self._remappings[label] = remapping
136 smach.logdebug("TRANSITIONS FOR %s: %s" % (label, str(self._transitions[label])))
137
138
139 if len(self._connector_outcomes) > 0 and self._last_added_label is not None:
140 for connector_outcome in self._connector_outcomes:
141 self._transitions[self._last_added_label][connector_outcome] = label
142
143 self._connector_outcomes = []
144 self._last_added_label = None
145
146 return state
147
148 @staticmethod
149 - def add_auto(label, state, connector_outcomes, transitions=None, remapping=None):
150 """Add a state to the state machine such that it automatically
151 transitions to the next added state.
152
153 Each state added will receive an additional transition from it
154 to the state which is added after it for every outcome given
155 via connector_outcomes.
156
157 @type label: string
158 @param label: The label of the state being added.
159
160 @param state: An instance of a class implementing the L{State} interface.
161
162 @type connector_outcomes: list of string
163 @param connector_outcomes: For which of the added state's outcomes a
164 transition to the next added state should be generated.
165
166 @param transitions: A dictionary mapping state outcomes to other state
167 labels. If one of these transitions follows the connector outcome
168 specified in the constructor, the provided transition will override
169 the automatically generated connector transition.
170 """
171
172 self = StateMachine._currently_opened_container()
173
174
175 add_ret = smach.StateMachine.add(label, state, transitions, remapping)
176
177
178 registered_outcomes = state.get_registered_outcomes()
179 if not all(co in registered_outcomes for co in connector_outcomes):
180 raise smach.InvalidStateError("Attempting to auto-connect states with outcomes %s, but state '%s' only has registerd outcomes: %s" % (str(connector_outcomes), str(label), str(registered_outcomes)))
181
182
183 self._last_added_label = label
184 self._connector_outcomes = connector_outcomes
185
186 return add_ret
187
188
190 if state_label is not None:
191
192 self._current_label = state_label
193 self._current_state = self._states[state_label]
194 self._current_transitions = self._transitions[state_label]
195 self._current_outcome = None
196 else:
197
198 self._current_label = None
199 self._current_state = None
200 self._current_transitions = None
201 self._current_outcome = None
202
204 """Method that updates the state machine once.
205 This checks if the current state is ready to transition, if so, it
206 requests the outcome of the current state, and then extracts the next state
207 label from the current state's transition dictionary, and then transitions
208 to the next state.
209 """
210 outcome = None
211 transition_target = None
212 last_state_label = self._current_label
213
214
215 if self._current_label not in self._states:
216 raise smach.InvalidStateError("State '%s' does not exist. Available states are: %s" %
217 (self._current_label, list(self._states.keys())))
218
219
220 if self.preempt_requested():
221 smach.loginfo("Preempt requested on state machine before executing the next state.")
222
223 if self._preempted_state is not None:
224
225 if self._preempted_state.preempt_requested():
226 smach.loginfo("Last state '%s' did not service preempt. Preempting next state '%s' before executing..." % (self._preempted_label, self._current_label))
227
228
229 self._preempt_current_state()
230 else:
231
232 self._preempt_requested = False
233 self._preempted_state = None
234 else:
235
236
237 self._preempt_current_state()
238
239
240 try:
241 self._state_transitioning_lock.release()
242 outcome = self._current_state.execute(
243 smach.Remapper(
244 self.userdata,
245 self._current_state.get_registered_input_keys(),
246 self._current_state.get_registered_output_keys(),
247 self._remappings[self._current_label]))
248 except smach.InvalidUserCodeError as ex:
249 smach.logerr("State '%s' failed to execute." % self._current_label)
250 raise ex
251 except:
252 raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
253 (self._current_label, self._current_state)
254 + traceback.format_exc())
255 finally:
256 self._state_transitioning_lock.acquire()
257
258
259 if outcome not in self._current_state.get_registered_outcomes():
260 raise smach.InvalidTransitionError(
261 "Attempted to return outcome '%s' from state '%s' of"
262 " type '%s' which only has registered outcomes: %s" %
263 (outcome,
264 self._current_label,
265 self._current_state,
266 self._current_state.get_registered_outcomes()))
267
268
269 if outcome not in self._current_transitions:
270 raise smach.InvalidTransitionError("Outcome '%s' of state '%s' is not bound to any transition target. Bound transitions include: %s" %
271 (str(outcome), str(self._current_label), str(self._current_transitions)))
272
273
274 transition_target = self._current_transitions[outcome]
275
276
277 if transition_target in self._states:
278
279 self._set_current_state(transition_target)
280
281
282 smach.loginfo("State machine transitioning '%s':'%s'-->'%s'" %
283 (last_state_label, outcome, transition_target))
284
285
286 self.call_transition_cbs()
287 else:
288
289
290 if self._preempt_requested and self._preempted_state is not None:
291 if not self._current_state.preempt_requested():
292 self.service_preempt()
293
294 if transition_target not in self.get_registered_outcomes():
295
296 transition_target = outcome
297
298 if transition_target in self.get_registered_outcomes():
299
300 self._set_current_state(None)
301
302
303 smach.loginfo("State machine terminating '%s':'%s':'%s'" %
304 (last_state_label, outcome, transition_target))
305
306
307 self.call_termination_cbs([last_state_label],transition_target)
308
309 return transition_target
310 else:
311 raise smach.InvalidTransitionError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." %
312 (outcome, self._current_label, transition_target))
313 return None
314
315
317 """Run the state machine on entry to this state.
318 This will set the "closed" flag and spin up the execute thread. Once
319 this flag has been set, it will prevent more states from being added to
320 the state machine.
321 """
322
323
324 with self._state_transitioning_lock:
325
326 try:
327 self.check_consistency()
328 except (smach.InvalidStateError, smach.InvalidTransitionError):
329 smach.logerr("Container consistency check failed.")
330 return None
331
332
333 self._is_running = True
334
335
336 self._preempted_label = None
337 self._preempted_state = None
338
339
340 self._set_current_state(self._initial_state_label)
341
342
343 self._copy_input_keys(parent_ud, self.userdata)
344
345
346 smach.loginfo("State machine starting in initial state '%s' with userdata: \n\t%s" %
347 (self._current_label, list(self.userdata.keys())))
348
349
350
351 self.call_start_cbs()
352
353
354 container_outcome = None
355
356
357 while container_outcome is None and self._is_running and not smach.is_shutdown():
358
359 container_outcome = self._update_once()
360
361
362 self._copy_output_keys(self.userdata, parent_ud)
363
364
365 self._is_running = False
366
367 return container_outcome
368
369
371 """Propagate preempt to currently active state.
372
373 This will attempt to preempt the currently active state.
374 """
375 with self._state_transitioning_lock:
376
377 self._preempt_requested = True
378
379 if self._current_state is not None:
380 self._preempt_current_state()
381
383 """Preempt the current state (might not be executing yet).
384 This also resets the preempt flag on a state that had previously received the preempt, but not serviced it."""
385 if self._preempted_state != self._current_state:
386 if self._preempted_state is not None:
387
388 self._preempted_state.recall_preempt()
389
390
391 self._preempted_state = self._current_state
392 self._preempted_label = self._current_label
393
394
395 try:
396 self._preempted_state.request_preempt()
397 except:
398 smach.logerr("Failed to preempt contained state '%s': %s" % (self._preempted_label, traceback.format_exc()))
399
400
403
405 if key not in self._states:
406 smach.logerr("Attempting to get state '%s' from StateMachine container. The only available states are: %s" % (key, str(list(self._states.keys()))))
407 raise KeyError()
408 return self._states[key]
409
411 smach.logdebug("Setting initial states to " + str(initial_states))
412
413 if len(initial_states) > 1:
414 smach.logwarn("Attempting to set initial state to include more than"
415 " one state, but the StateMachine container can only"
416 " have one initial state. Taking the first one.")
417
418
419 if len(initial_states) > 0:
420 self._initial_state_label = initial_states[0]
421
422 self.userdata.update(userdata)
423
425 return [str(self._current_label)]
426
428 return [str(self._initial_state_label)]
429
431 int_edges = []
432 for (from_label,transitions) in ((k,self._transitions[k]) for k in self._transitions):
433 for (outcome,to_label) in ((k,transitions[k]) for k in transitions):
434 int_edges.append((outcome, from_label, to_label))
435 return int_edges
436
437
439 """Validate full state specification (label, state, and transitions).
440 This checks to make sure the required variables are in the state spec,
441 as well as verifies that all outcomes referenced in the transitions
442 are registered as valid outcomes in the state object. If a state
443 specification fails validation, a L{smach.InvalidStateError} is
444 thrown.
445 """
446
447 registered_outcomes = state.get_registered_outcomes()
448 for outcome in transitions:
449 if outcome not in registered_outcomes:
450 raise smach.InvalidTransitionError("Specified outcome '"+outcome+"' on state '"+label+"', which only has available registered outcomes: "+str(registered_outcomes))
451
453 """Check the entire state machine for consistency.
454 This asserts that all transition targets are states that are in the
455 state machine. If this fails, it raises an L{InvalidTransitionError}
456 with relevant information.
457 """
458
459 available_states = set(list(self._states.keys())+list(self.get_registered_outcomes()))
460
461
462 registered_sm_outcomes = self.get_registered_outcomes()
463
464
465 errors = ""
466
467
468 if self._initial_state_label is None:
469 errors = errors + "\n\tNo initial state set."
470 elif self._initial_state_label not in self._states:
471 errors = errors + "\n\tInitial state label: '"+str(self._initial_state_label)+"' is not in the state machine."
472
473
474 state_specs = [(label, self._states[label], self._transitions[label])
475 for label in self._states]
476
477 for label,state,transitions in state_specs:
478
479 transition_states = set([s for s in transitions.values()
480 if s is not None and s != ''])
481
482 missing_states = transition_states.difference(available_states)
483
484
485 if len(missing_states) > 0:
486 errors = (errors
487 + "\n\tState '" + str(label)
488 + "' references unknown states: " + str(list(missing_states)))
489
490
491 terminal_outcomes = set([o for (o, s) in ((k, transitions[k])
492 for k in transitions)
493 if s is None or s == ''])
494
495 missing_outcomes = terminal_outcomes.difference(registered_sm_outcomes)
496
497 if len(missing_outcomes) > 0:
498 errors = (errors
499 + "\n\tState '" + str(label)
500 + "' references unregistered outcomes: " + str(list(missing_outcomes)))
501
502
503 if len(errors) > 0:
504 raise smach.InvalidTransitionError("State machine failed consistency check: "+errors+"\n\n\tAvailable states: "+str(list(available_states)))
505
506
508 """Returns true if the state machine is running."""
509 return self._is_running
510