asmach.state

13 """StateMachine 14 15 This is a finite state machine smach container. Note that though this is 16 a state machine, it also implements the L{smach.State} 17 interface, so these can be composed hierarchically, if such a pattern is 18 desired. 19 20 States are added to the state machine as 3-tuple specifications: 21 - label 22 - state instance 23 - transitions 24 25 The label is a string, the state instance is any class that implements the 26 L{smach.State} interface, and transitions is a dictionary mapping strings onto 27 strings which represent the transitions out of this new state. Transitions 28 can take one of three forms: 29 - OUTCOME -> STATE_LABEL 30 - OUTCOME -> None (or unspecified) 31 - OUTCOME -> SM_OUTCOME 32 """

33 - def __init__(self, outcomes, input_keys=[], output_keys=[]):

34 """Constructor for smach StateMachine Container. 35 36 @type outcomes: list of strings 37 @param outcomes: The potential outcomes of this state machine. 38 """ 39 40 # Call super's constructor 41 smach.container.Container.__init__(self, outcomes, input_keys, output_keys) 42 43 # Properties 44 self._state_transitioning_lock = threading.Lock() 45 46 # Current state of the state machine 47 self._is_running = False # True when a goal has been dispatched to and accepted by the state machine 48 49 self._initial_state_label = None 50 51 self._current_label = None 52 self._current_state = None 53 self._current_transitions = None 54 self._current_outcome = None 55 56 # The label of the last preempted state 57 self._preempted_label = None 58 self._preempted_state = None 59 60 # State machine storage 61 # These are dictionaries of State objects and transition dictionaries 62 # keyed on unique labels 63 self._states = {} 64 self._transitions = {} 65 self._remappings = {} 66 67 # Construction vars 68 self._last_added_label = None 69 self._connector_outcomes = [] 70 71 # Thread for execution of state switching 72 self._execute_thread = None 73 self.userdata = smach.UserData()

74 75 ### Construction methods 76 @staticmethod

77 - def add(label, state, transitions = None, remapping = None):

78 """Add a state to the opened state machine. 79 80 @type label: string 81 @param label: The label of the state being added. 82 83 @param state: An instance of a class implementing the L{State} interface. 84 85 @param transitions: A dictionary mapping state outcomes to other state 86 labels or container outcomes. 87 88 @param remapping: A dictrionary mapping local userdata keys to userdata 89 keys in the container. 90 """ 91 # Get currently opened container 92 self = StateMachine._currently_opened_container() 93 94 smach.logdebug('Adding state (%s, %s, %s)' % (label, str(state), str(transitions))) 95 96 # Set initial state if it is still unset 97 if self._initial_state_label is None: 98 self._initial_state_label = label 99 100 if transitions is None: 101 transitions = {} 102 103 if remapping is None: 104 remapping = {} 105 106 # Add group transitions to this new state, if they exist 107 """ 108 if 'transitions' in smach.Container._context_kwargs: 109 for outcome, target in smach.Container._context_kwargs['transitions'].iteritems(): 110 if outcome not in transitions: 111 transitions[outcome] = target 112 """ 113 114 # Check the state specification 115 self.check_state_spec(label, state, transitions) 116 117 # Check if th label already exists 118 if label in self._states: 119 raise smach.InvalidStateError( 120 'Attempting to add state with label "'+label+'" to state machine, but this label is already being used.') 121 122 # Debug info 123 smach.logdebug("Adding state '"+str(label)+"' to the state machine.") 124 125 # Create implicit terminal transitions, and combine them with the explicit transitions 126 registered_outcomes = state.get_registered_outcomes() 127 128 # Get a list of the unbound transitions 129 missing_transitions = dict([(o,None) for o in registered_outcomes if o not in transitions.keys()]) 130 transitions.update(missing_transitions) 131 smach.logdebug("State '%s' is missing transitions: %s" % (label,str(missing_transitions))) 132 133 # Add state and transitions to the dictionary 134 self._states[label] = state 135 self._transitions[label] = transitions 136 self._remappings[label] = remapping 137 smach.logdebug("TRANSITIONS FOR %s: %s" % (label, str(self._transitions[label]))) 138 139 # Add transition to this state if connected outcome is defined 140 if len(self._connector_outcomes) > 0 and self._last_added_label is not None: 141 for connector_outcome in self._connector_outcomes: 142 self._transitions[self._last_added_label][connector_outcome] = label 143 # Reset connector outcomes and last added label 144 self._connector_outcomes = [] 145 self._last_added_label = None 146 147 return state

148 149 @staticmethod

150 - def add_auto(label, state, connector_outcomes, transitions = None, remapping = None):

151 """Add a state to the state machine such that it automatically transitions to the next added state. 152 Each state added will receive an additional transition from it to the 153 state which is added after it. The transition will follow the outcome 154 specified at construction of this container. 155 156 @type label: string 157 @param label: The label of the state being added. 158 159 @param state: An instance of a class implementing the L{State} interface. 160 161 @param transitions: A dictionary mapping state outcomes to other state 162 labels. If one of these transitions follows the connector outcome 163 specified in the constructor, the provided transition will override 164 the automatically generated connector transition. 165 """ 166 # Get currently opened container 167 self = StateMachine._currently_opened_container() 168 169 # First add this state 170 add_ret = smach.StateMachine.add(label, state, transitions, remapping) 171 172 # Make sure the connector outcomes are valid for this state 173 registered_outcomes = state.get_registered_outcomes() 174 if not all([co in registered_outcomes for co in connector_outcomes]): 175 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))) 176 177 # Store this state as the last state and store the connector outcomes 178 self._last_added_label = label 179 self._connector_outcomes = connector_outcomes 180 181 return add_ret

182 183 ### Internals

184 - def _set_current_state(self, state_label):

185 if state_label is not None: 186 # Store the current label and states 187 self._current_label = state_label 188 self._current_state = self._states[state_label] 189 self._current_transitions = self._transitions[state_label] 190 self._current_outcome = None 191 else: 192 # Store the current label and states 193 self._current_label = None 194 self._current_state = None 195 self._current_transitions = None 196 self._current_outcome = None

197 198 @async.function

199 - def _update_once(self):

200 """Method that updates the state machine once. 201 This checks if the current state is ready to transition, if so, it 202 requests the outcome of the current state, and then extracts the next state 203 label from the current state's transition dictionary, and then transitions 204 to the next state. 205 """ 206 yield 207 outcome = None 208 transition_target = None 209 last_state_label = self._current_label 210 211 # Make sure the state exists 212 if self._current_label not in self._states.keys(): 213 raise smach.InvalidStateError("State '%s' does not exist. Available states are: %s" % 214 (str(self._current_label),str(self._states.keys()))) 215 216 # Check if a preempt was requested before or while the last state was running 217 if self.preempt_requested(): 218 smach.loginfo("Preempt requested on state machine before executing the next state.") 219 # We were preempted 220 if self._preempted_state is not None: 221 # We were preempted while the last state was running 222 if self._preempted_state.preempt_requested(): 223 smach.loginfo("Last state '%s' did not service preempt. Preempting next state '%s' before executing..." % (self._preempted_label, self._current_label)) 224 # The flag was not reset, so we need to keep preempting 225 # (this will reset the current preempt) 226 self._preempt_current_state() 227 else: 228 # The flag was reset, so the container can reset 229 self._preempt_requested = False 230 else: 231 # We were preempted after the last state was running 232 # So we should preempt this state before we execute it 233 self._preempt_current_state() 234 235 # Execute the state 236 try: 237 self._state_transitioning_lock.release() 238 outcome = yield self._current_state.execute_async( 239 smach.Remapper( 240 self.userdata, 241 self._current_state.get_registered_input_keys(), 242 self._current_state.get_registered_output_keys(), 243 self._remappings[self._current_label])) 244 except smach.InvalidUserCodeError as ex: 245 smach.logerr("State '%s' failed to execute." % self._current_label) 246 raise ex 247 except: 248 raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " % ( self._current_label, self._current_state ) + traceback.format_exc()) 249 finally: 250 self._state_transitioning_lock.acquire() 251 252 # Check if outcome was a potential outcome for this type of state 253 if outcome not in self._current_state.get_registered_outcomes(): 254 raise smach.InvalidTransitionError("Attempted to return outcome '%s' from state '%s' of type '%s' which only has registered outcomes: %s" % 255 ( str(outcome), 256 str(self._current_label), 257 str(self._current_state), 258 str(self._current_state.get_registered_outcomes()))) 259 260 # Check if this outcome is actually mapped to any target 261 if outcome not in self._current_transitions: 262 raise smach.InvalidTransitionError("Outcome '%s' of state '%s' is not bound to any transition target. Bound transitions include: %s" % 263 (str(outcome), str(self._current_label), str(self._current_transitions))) 264 265 # Set the transition target 266 transition_target = self._current_transitions[outcome] 267 268 # Check if the transition target is a state in this state machine, or an outcome of this state machine 269 if transition_target in self._states: 270 # Set the new state 271 self._set_current_state(transition_target) 272 273 # Spew some info 274 smach.loginfo("State machine transitioning '%s':'%s'-->'%s'" % (str(last_state_label), str(outcome), str(transition_target))) 275 276 # Call transition callbacks 277 self.call_transition_cbs() 278 else: 279 # This is a terminal state 280 281 if self._preempt_requested and self._preempted_state is not None: 282 if not self._current_state.preempt_requested(): 283 self.service_preempt() 284 285 if transition_target not in self.get_registered_outcomes(): 286 # This is a container outcome that will fall through 287 transition_target = outcome 288 289 if transition_target in self.get_registered_outcomes(): 290 # The transition target is an outcome of the state machine 291 self._set_current_state(None) 292 293 # Spew some info 294 smach.loginfo("State machine terminating '%s':'%s':'%s'" % (str(last_state_label), str(outcome), str(transition_target))) 295 296 # Call termination callbacks 297 self.call_termination_cbs([last_state_label],transition_target) 298 299 async.returnValue(transition_target) 300 else: 301 raise smach.InvalidTransitionError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." % 302 (str(outcome), str(self._current_label), str(transition_target))) 303 async.returnValue(None)

304 305 ### State Interface 306 @async.function

307 - def execute_async(self, parent_ud = smach.UserData()):

308 """Run the state machine on entry to this state. 309 This will set the "closed" flag and spin up the execute thread. Once 310 this flag has been set, it will prevent more states from being added to 311 the state machine. 312 """ 313 314 # This will prevent preempts from getting propagated to non-existent children 315 with self._state_transitioning_lock: 316 # Check state consistency 317 try: 318 self.check_consistency() 319 except (smach.InvalidStateError, smach.InvalidTransitionError): 320 smach.logerr("Container consistency check failed.") 321 async.returnValue(None) 322 323 # Set running flag 324 self._is_running = True 325 326 # Initialize preempt state 327 self._preempted_label = None 328 self._preempted_state = None 329 330 # Set initial state 331 self._set_current_state(self._initial_state_label) 332 333 # Copy input keys 334 self._copy_input_keys(parent_ud, self.userdata) 335 336 # Spew some info 337 smach.loginfo("State machine starting in initial state '%s' with userdata: \n\t%s" % 338 (self._current_label,str(self.userdata.keys()))) 339 340 341 # Call start callbacks 342 self.call_start_cbs() 343 344 # Initialize container outcome 345 container_outcome = None 346 347 # Step through state machine 348 while container_outcome is None and self._is_running and not smach.is_shutdown(): 349 # Update the state machine 350 container_outcome = yield self._update_once() 351 352 # Copy output keys 353 self._copy_output_keys(self.userdata, parent_ud) 354 355 # We're no longer running 356 self._is_running = False 357 358 async.returnValue(container_outcome)

359 360 ## Preemption management

361 - def request_preempt(self):

362 """Propagate preempt to currently active state. 363 364 This will attempt to preempt the currently active state. 365 """ 366 with self._state_transitioning_lock: 367 # Aleways Set this container's preempted flag 368 self._preempt_requested = True 369 # Only propagate preempt if the current state is defined 370 if self._current_state is not None: 371 self._preempt_current_state()

372

373 - def _preempt_current_state(self):

374 """Preempt the current state (might not be executing yet). 375 This also resets the preempt flag on a state that had previously received the preempt, but not serviced it.""" 376 if self._preempted_state != self._current_state: 377 if self._preempted_state is not None: 378 # Reset the previously preempted state (that has now terminated) 379 self._preempted_state.recall_preempt() 380 381 # Store the label of the currently active state 382 self._preempted_state = self._current_state 383 self._preempted_label = self._current_label 384 385 # Request the currently active state to preempt 386 try: 387 self._preempted_state.request_preempt() 388 except: 389 smach.logerr("Failed to preempt contained state '%s': %s" % (self._preempted_label, traceback.format_exc()))

390 391 ### Container interface

392 - def get_children(self):

393 return self._states

394

395 - def __getitem__(self,key):

396 if key not in self._states: 397 smach.logerr("Attempting to get state '%s' from StateMachine container. The only available states are: %s" % (key, self._states.keys())) 398 raise KeyError() 399 return self._states[key]

400

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

402 smach.logdebug("Setting initial state to "+str(initial_states)) 403 404 if len(initial_states) > 1: 405 smach.logwarn("Attempting to set initial state to include more than one state, but the StateMachine container can only have one initial state.") 406 407 # Set the initial state label 408 if len(initial_states) > 0: 409 self._initial_state_label = initial_states[0] 410 # Set local userdata 411 self.userdata.update(userdata)

412

413 - def get_active_states(self):

414 return [str(self._current_label)]

415

416 - def get_initial_states(self):

417 return [str(self._initial_state_label)]

418

419 - def get_internal_edges(self):

420 int_edges = [] 421 for (from_label,transitions) in self._transitions.iteritems(): 422 for (outcome,to_label) in transitions.iteritems(): 423 int_edges.append([outcome,from_label,to_label]) 424 return int_edges

425 426 ### Validation methods

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

428 """Validate full state specification (label, state, and transitions). 429 This checks to make sure the required variables are in the state spec, 430 as well as verifies that all outcomes referenced in the transitions 431 are registered as valid outcomes in the state object. If a state 432 specification fails validation, a L{smach.InvalidStateError} is 433 thrown. 434 """ 435 # Make sure all transitions are from registered outcomes of this state 436 registered_outcomes = state.get_registered_outcomes() 437 for outcome in transitions.keys(): 438 if outcome not in registered_outcomes: 439 raise smach.InvalidTransitionError("Specified outcome '"+outcome+"' on state '"+label+"', which only has available registered outcomes: "+str(registered_outcomes))

440

441 - def check_consistency(self):

442 """Check the entire state machine for consistency. 443 This asserts that all transition targets are states that are in the 444 state machine. If this fails, it raises an L{InvalidTransitionError} 445 with relevant information. 446 """ 447 # Construct a set of available states 448 available_states = set(self._states.keys()+list(self.get_registered_outcomes())) 449 450 # Grab the registered outcomes for the state machine 451 registered_sm_outcomes = self.get_registered_outcomes() 452 453 # Hopefully this string stays empty 454 errors = "" 455 456 # Check initial_state_label 457 if self._initial_state_label is None: 458 errors = errors + "\n\tNo initial state set." 459 elif self._initial_state_label not in self._states.keys(): 460 errors = errors + "\n\tInitial state label: '"+str(self._initial_state_label)+"' is not in the state machine." 461 462 # Generate state specifications 463 state_specs = [ 464 (label, self._states[label], self._transitions[label]) 465 for label in self._states.keys()] 466 # Iterate over all states 467 for label,state,transitions in state_specs: 468 # Check that all potential outcomes are registered in this state 469 transition_states = set( 470 [s for s in transitions.values() if ((s is not None) and (s != ''))] ) 471 # Generate a list of missing states 472 missing_states = transition_states.difference(available_states) 473 474 # Check number of missing states 475 if len(missing_states) > 0: 476 errors = (errors 477 +"\n\tState '"+str(label) 478 +"' references unknown states: "+str(list(missing_states))) 479 480 # Check terminal outcomes for this state 481 terminal_outcomes = set([o for o,s in transitions.iteritems() if ((s is None) or (s == ''))]) 482 # Terminal outcomes should be in the registered outcomes of this state machine 483 missing_outcomes = terminal_outcomes.difference(registered_sm_outcomes) 484 # Check number of missing outcomes 485 if len(missing_outcomes) > 0: 486 errors = (errors 487 +"\n\tState '"+str(label) 488 +"' references unregistered outcomes: "+str(list(missing_outcomes))) 489 490 # Check errors 491 if len(errors) > 0: 492 raise smach.InvalidTransitionError("State machine failed consistency check: "+errors+"\n\n\tAvailable states: "+str(list(available_states)))

493

494 - def set_userdata(self,userdata):

495 """Propagate an updated userdata structure into this state machine.""" 496 # Update the container's userdata 497 smach.Container.set_userdata(self, userdata) 498 # Update the userdata of the contained states 499 for state in self._states.values(): 500 state.set_userdata(self.userdata)

501 502 ### Introspection methods

503 - def is_running(self):

504 """async.returnValues true if the state machine is running.""" 505 return self._is_running

Source Code for Module asmach.state_machine