smach.state_machine

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 # Call super's constructor 40 smach.container.Container.__init__(self, outcomes, input_keys, output_keys) 41 42 # Properties 43 self._state_transitioning_lock = threading.Lock() 44 45 # Current state of the state machine 46 self._is_running = False # True when a goal has been dispatched to and accepted by the state machine 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 # The label of the last preempted state 56 self._preempted_label = None 57 self._preempted_state = None 58 59 # State machine storage 60 # These are dictionaries of State objects and transition dictionaries 61 # keyed on unique labels 62 self._states = {} 63 self._transitions = {} 64 self._remappings = {} 65 66 # Construction vars 67 self._last_added_label = None 68 self._connector_outcomes = [] 69 70 # Thread for execution of state switching 71 self._execute_thread = None 72 self.userdata = smach.UserData()

73 74 ### Construction methods 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 # Get currently opened container 91 self = StateMachine._currently_opened_container() 92 93 smach.logdebug('Adding state (%s, %s, %s)' % (label, str(state), str(transitions))) 94 95 # Set initial state if it is still unset 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 # Add group transitions to this new state, if they exist 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 # Check the state specification 114 self.check_state_spec(label, state, transitions) 115 116 # Check if th label already exists 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 # Debug info 122 smach.logdebug("Adding state '"+str(label)+"' to the state machine.") 123 124 # Create implicit terminal transitions, and combine them with the explicit transitions 125 registered_outcomes = state.get_registered_outcomes() 126 127 # Get a list of the unbound transitions 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 # Add state and transitions to the dictionary 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 # Add transition to this state if connected outcome is defined 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 # Reset connector outcomes and last added label 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 # Get currently opened container 172 self = StateMachine._currently_opened_container() 173 174 # First add this state 175 add_ret = smach.StateMachine.add(label, state, transitions, remapping) 176 177 # Make sure the connector outcomes are valid for this state 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 # Store this state as the last state and store the connector outcomes 183 self._last_added_label = label 184 self._connector_outcomes = connector_outcomes 185 186 return add_ret

187 188 ### Internals

189 - def _set_current_state(self, state_label):

190 if state_label is not None: 191 # Store the current label and states 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 # Store the current label and states 198 self._current_label = None 199 self._current_state = None 200 self._current_transitions = None 201 self._current_outcome = None

202

203 - def _update_once(self):

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 # Make sure the state exists 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 # Check if a preempt was requested before or while the last state was running 220 if self.preempt_requested(): 221 smach.loginfo("Preempt requested on state machine before executing the next state.") 222 # We were preempted 223 if self._preempted_state is not None: 224 # We were preempted while the last state was running 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 # The flag was not reset, so we need to keep preempting 228 # (this will reset the current preempt) 229 self._preempt_current_state() 230 else: 231 # The flag was reset, so the container can reset 232 self._preempt_requested = False 233 self._preempted_state = None 234 else: 235 # We were preempted after the last state was running 236 # So we should preempt this state before we execute it 237 self._preempt_current_state() 238 239 # Execute the state 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 # Check if outcome was a potential outcome for this type of state 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 # Check if this outcome is actually mapped to any target 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 # Set the transition target 274 transition_target = self._current_transitions[outcome] 275 276 # Check if the transition target is a state in this state machine, or an outcome of this state machine 277 if transition_target in self._states: 278 # Set the new state 279 self._set_current_state(transition_target) 280 281 # Spew some info 282 smach.loginfo("State machine transitioning '%s':'%s'-->'%s'" % 283 (last_state_label, outcome, transition_target)) 284 285 # Call transition callbacks 286 self.call_transition_cbs() 287 else: 288 # This is a terminal state 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 # This is a container outcome that will fall through 296 transition_target = outcome 297 298 if transition_target in self.get_registered_outcomes(): 299 # The transition target is an outcome of the state machine 300 self._set_current_state(None) 301 302 # Spew some info 303 smach.loginfo("State machine terminating '%s':'%s':'%s'" % 304 (last_state_label, outcome, transition_target)) 305 306 # Call termination callbacks 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 ### State Interface

316 - def execute(self, parent_ud = smach.UserData()):

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 # This will prevent preempts from getting propagated to non-existent children 324 with self._state_transitioning_lock: 325 # Check state consistency 326 try: 327 self.check_consistency() 328 except (smach.InvalidStateError, smach.InvalidTransitionError): 329 smach.logerr("Container consistency check failed.") 330 return None 331 332 # Set running flag 333 self._is_running = True 334 335 # Initialize preempt state 336 self._preempted_label = None 337 self._preempted_state = None 338 339 # Set initial state 340 self._set_current_state(self._initial_state_label) 341 342 # Copy input keys 343 self._copy_input_keys(parent_ud, self.userdata) 344 345 # Spew some info 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 # Call start callbacks 351 self.call_start_cbs() 352 353 # Initialize container outcome 354 container_outcome = None 355 356 # Step through state machine 357 while container_outcome is None and self._is_running and not smach.is_shutdown(): 358 # Update the state machine 359 container_outcome = self._update_once() 360 361 # Copy output keys 362 self._copy_output_keys(self.userdata, parent_ud) 363 364 # We're no longer running 365 self._is_running = False 366 367 return container_outcome

368 369 ## Preemption management

370 - def request_preempt(self):

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 # Aleways Set this container's preempted flag 377 self._preempt_requested = True 378 # Only propagate preempt if the current state is defined 379 if self._current_state is not None: 380 self._preempt_current_state()

381

382 - def _preempt_current_state(self):

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 # Reset the previously preempted state (that has now terminated) 388 self._preempted_state.recall_preempt() 389 390 # Store the label of the currently active state 391 self._preempted_state = self._current_state 392 self._preempted_label = self._current_label 393 394 # Request the currently active state to preempt 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 ### Container interface

401 - def get_children(self):

402 return self._states

403

404 - def __getitem__(self,key):

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

410 - def set_initial_state(self, initial_states, userdata=smach.UserData()):

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 # Set the initial state label 419 if len(initial_states) > 0: 420 self._initial_state_label = initial_states[0] 421 # Set local userdata 422 self.userdata.update(userdata)

423

424 - def get_active_states(self):

425 return [str(self._current_label)]

426

427 - def get_initial_states(self):

428 return [str(self._initial_state_label)]

429

430 - def get_internal_edges(self):

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 ### Validation methods

438 - def check_state_spec(self, label, state, transitions):

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 # Make sure all transitions are from registered outcomes of this state 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

452 - def check_consistency(self):

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 # Construct a set of available states 459 available_states = set(list(self._states.keys())+list(self.get_registered_outcomes())) 460 461 # Grab the registered outcomes for the state machine 462 registered_sm_outcomes = self.get_registered_outcomes() 463 464 # Hopefully this string stays empty 465 errors = "" 466 467 # Check initial_state_label 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 # Generate state specifications 474 state_specs = [(label, self._states[label], self._transitions[label]) 475 for label in self._states] 476 # Iterate over all states 477 for label,state,transitions in state_specs: 478 # Check that all potential outcomes are registered in this state 479 transition_states = set([s for s in transitions.values() 480 if s is not None and s != '']) 481 # Generate a list of missing states 482 missing_states = transition_states.difference(available_states) 483 484 # Check number of missing states 485 if len(missing_states) > 0: 486 errors = (errors 487 + "\n\tState '" + str(label) 488 + "' references unknown states: " + str(list(missing_states))) 489 490 # Check terminal outcomes for this state 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 # Terminal outcomes should be in the registered outcomes of this state machine 495 missing_outcomes = terminal_outcomes.difference(registered_sm_outcomes) 496 # Check number of missing outcomes 497 if len(missing_outcomes) > 0: 498 errors = (errors 499 + "\n\tState '" + str(label) 500 + "' references unregistered outcomes: " + str(list(missing_outcomes))) 501 502 # Check errors 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 ### Introspection methods

507 - def is_running(self):

508 """Returns true if the state machine is running.""" 509 return self._is_running

Source Code for Module smach.state_machine