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

181 182 ### Internals

183 - def _set_current_state(self, state_label):

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

196

197 - def _update_once(self):

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

302 303 ### State Interface

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

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

356 357 ## Preemption management

358 - def request_preempt(self):

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

369

370 - def _preempt_current_state(self):

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

387 388 ### Container interface

389 - def get_children(self):

390 return self._states

391

392 - def __getitem__(self,key):

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

397

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

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

409

410 - def get_active_states(self):

411 return [str(self._current_label)]

412

413 - def get_initial_states(self):

414 return [str(self._initial_state_label)]

415

416 - def get_internal_edges(self):

417 int_edges = [] 418 for (from_label,transitions) in self._transitions.iteritems(): 419 for (outcome,to_label) in transitions.iteritems(): 420 int_edges.append([outcome,from_label,to_label]) 421 return int_edges

422 423 ### Validation methods

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

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

437

438 - def check_consistency(self):

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

490

491 - def set_userdata(self,userdata):

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

498 499 ### Introspection methods

500 - def is_running(self):

501 """Returns true if the state machine is running.""" 502 return self._is_running

Source Code for Module smach.state_machine