def __getitem__(self, key): if key != self._state_label: smach.logerr( "Attempting to get state '%s' from Iterator container. The only available state is '%s'." % (key, self._state_label)) raise KeyError() return self._state
def __setitem__(self, key, item): if key not in self._output: smach.logerr( "Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s." % (key, self._output)) return self._ud.__setitem__(self._remap(key), item)
def __setattr__(self, name, value): if name[0] == '_' or not self.__dict__.has_key('_Remapper__initialized'): return object.__setattr__(self, name, value) if name not in self._output: smach.logerr("Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s." % (name, self._output)) return None setattr(self._ud, self._remap(name), value)
def __exit__(self, exc_type, exc_val, exc_tb): if exc_type is None: return self.close() else: if exc_type != smach.InvalidStateError and exc_type != smach.InvalidTransitionError: smach.logerr( "Error raised during SMACH container construction: \n" + "\n".join( traceback.format_exception(exc_type, exc_val, exc_tb)))
def execute(self, parent_ud): self._is_running = True # Copy input keys self._copy_input_keys(parent_ud, self.userdata) self.call_start_cbs() # Iterate over items outcome = self._exhausted_outcome if hasattr(self._items, '__call__'): it = self._items().__iter__() else: it = self._items.__iter__() while not smach.is_shutdown(): try: item = it.next() except: outcome = self._exhausted_outcome break smach.loginfo("Iterating %s of %s" % (str(item), str(self._items))) self.userdata[self._items_label] = item # Enter the contained state try: outcome = self._state.execute(self.userdata) except smach.InvalidUserCodeError as ex: smach.logerr("Could not execute Iterator state '%s'" % self._state_label) raise ex except: raise smach.InvalidUserCodeError( "Could not execute iterator state '%s' of type '%s': " % (self._state_label, self._state) + traceback.format_exc()) # Check if we should stop preemptively if self._preempt_requested\ or outcome in self._break_outcomes\ or (len(self._loop_outcomes) > 0 and outcome not in self._loop_outcomes): self._preempt_requested = False break self.call_transition_cbs() # Remap the outcome if necessary if outcome in self._final_outcome_map: outcome = self._final_outcome_map[outcome] # Copy output keys self._copy_output_keys(self.userdata, parent_ud) self._is_running = False self.call_termination_cbs(self._state_label, outcome) return outcome
def __setattr__(self, name, value): if name[0] == '_' or not self.__dict__.has_key( '_Remapper__initialized'): return object.__setattr__(self, name, value) if name not in self._output: smach.logerr( "Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s." % (name, self._output)) return None setattr(self._ud, self._remap(name), value)
def execute(self, parent_ud): self._is_running = True # Copy input keys self._copy_input_keys(parent_ud, self.userdata) self.call_start_cbs() # Iterate over items outcome = self._exhausted_outcome if hasattr(self._items,'__call__'): it = self._items().__iter__() else: it = self._items.__iter__() while not smach.is_shutdown(): try: item = it.next() except: outcome = self._exhausted_outcome break smach.loginfo("Iterating %s of %s" % (str(item), str(self._items))) self.userdata[self._items_label] = item # Enter the contained state try: outcome = self._state.execute(self.userdata) except smach.InvalidUserCodeError as ex: smach.logerr("Could not execute Iterator state '%s'" % self._state_label) raise ex except: raise smach.InvalidUserCodeError("Could not execute iterator state '%s' of type '%s': " % ( self._state_label, self._state) + traceback.format_exc()) # Check if we should stop preemptively if self._preempt_requested\ or outcome in self._break_outcomes\ or (len(self._loop_outcomes) > 0 and outcome not in self._loop_outcomes): self._preempt_requested = False break self.call_transition_cbs() # Remap the outcome if necessary if outcome in self._final_outcome_map: outcome = self._final_outcome_map[outcome] # Copy output keys self._copy_output_keys(self.userdata, parent_ud) self._is_running = False self.call_termination_cbs(self._state_label,outcome) return outcome
def extract(self, keys, remapping): ud = UserData() reverse_remapping = dict([(v,k) for (k,v) in remapping.iteritems()]) if len(reverse_remapping) != len(remapping): smach.logerr("SMACH userdata remapping is not one-to-one: " + str(remapping)) for k in keys: rmk = k if k in reverse_remapping: rmk = reverse_remapping[k] ud[rmk] = copy.copy(self[k]) return ud
def call_transition_cbs(self): """Calls the registered transition callbacks. Callback functions are called with two arguments in addition to any user-supplied arguments: - userdata - a list of active states """ try: for (cb,args) in self._transition_cbs: cb(self.userdata, self.get_active_states(), *args) except: smach.logerr("Could not execute transition callback: "+traceback.format_exc())
def extract(self, keys, remapping): ud = UserData() reverse_remapping = dict([(v, k) for (k, v) in remapping.iteritems()]) if len(reverse_remapping) != len(remapping): smach.logerr("SMACH userdata remapping is not one-to-one: " + str(remapping)) for k in keys: rmk = k if k in reverse_remapping: rmk = reverse_remapping[k] ud[rmk] = copy.copy(self[k]) return ud
def execute_async(self, parent_ud = smach.UserData()): """Run the state machine on entry to this state. This will set the "closed" flag and spin up the execute thread. Once this flag has been set, it will prevent more states from being added to the state machine. """ # This will prevent preempts from getting propagated to non-existent children with self._state_transitioning_lock: # Check state consistency try: self.check_consistency() except (smach.InvalidStateError, smach.InvalidTransitionError): smach.logerr("Container consistency check failed.") async.returnValue(None) # Set running flag self._is_running = True # Initialize preempt state self._preempted_label = None self._preempted_state = None # Set initial state self._set_current_state(self._initial_state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo("State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label,str(self.userdata.keys()))) # Call start callbacks self.call_start_cbs() # Initialize container outcome container_outcome = None # Step through state machine while container_outcome is None and self._is_running and not smach.is_shutdown(): # Update the state machine container_outcome = yield self._update_once() # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False async.returnValue(container_outcome)
def call_transition_cbs(self): """Calls the registered transition callbacks. Callback functions are called with two arguments in addition to any user-supplied arguments: - userdata - a list of active states """ try: for (cb, args) in self._transition_cbs: cb(self.userdata, self.get_active_states(), *args) except: smach.logerr("Could not execute transition callback: " + traceback.format_exc())
def call_termination_cbs(self, terminal_states, outcome): """Calls the registered termination callbacks. Callback functions are called with three arguments in addition to any user-supplied arguments: - userdata - a list of terminal states - the outcome of this container """ try: for (cb,args) in self._termination_cbs: cb(self.userdata, terminal_states, outcome, *args) except: smach.logerr("Could not execute termination callback: "+traceback.format_exc())
def call_termination_cbs(self, terminal_states, outcome): """Calls the registered termination callbacks. Callback functions are called with three arguments in addition to any user-supplied arguments: - userdata - a list of terminal states - the outcome of this container """ try: for (cb, args) in self._termination_cbs: cb(self.userdata, terminal_states, outcome, *args) except: smach.logerr("Could not execute termination callback: " + traceback.format_exc())
def __getattr__(self, name): """Overload getattr to be thread safe.""" if name[0] == '_': return object.__getattr__(self, name) if not name in self._locks.keys(): self._locks[name] = threading.Lock() try: with self._locks[name]: temp = self._data[name] except: smach.logerr("Userdata key '%s' not available. Available keys are: %s" % (name, self._data.keys())) raise KeyError() return temp
def close(self): """Close the container.""" # Make sure this container is the currently open container if len(Container._construction_stack) > 0: if self != Container._construction_stack[-1]: raise smach.InvalidStateError('Attempting to close a container that is not currently open.') # Pop this container off the construction stack Container._construction_stack.pop() Container._construction_lock.release() # Check consistency of container, post-construction try: self.check_consistency() except (smach.InvalidStateError, smach.InvalidTransitionError): smach.logerr("Container consistency check failed.")
def __getattr__(self, name): """Overload getattr to be thread safe.""" if name[0] == '_': return object.__getattr__(self, name) if not name in self._locks.keys(): self._locks[name] = threading.Lock() try: with self._locks[name]: temp = self._data[name] except: smach.logerr( "Userdata key '%s' not available. Available keys are: %s" % (name, self._data.keys())) raise KeyError() return temp
def _preempt_current_state(self): """Preempt the current state (might not be executing yet). This also resets the preempt flag on a state that had previously received the preempt, but not serviced it.""" if self._preempted_state != self._current_state: if self._preempted_state is not None: # Reset the previously preempted state (that has now terminated) self._preempted_state.recall_preempt() # Store the label of the currently active state self._preempted_state = self._current_state self._preempted_label = self._current_label # Request the currently active state to preempt try: self._preempted_state.request_preempt() except: smach.logerr("Failed to preempt contained state '%s': %s" % (self._preempted_label, traceback.format_exc()))
def close(self): """Close the container.""" # Make sure this container is the currently open container if len(Container._construction_stack) > 0: if self != Container._construction_stack[-1]: raise smach.InvalidStateError( 'Attempting to close a container that is not currently open.' ) # Pop this container off the construction stack Container._construction_stack.pop() Container._construction_lock.release() # Check consistency of container, post-construction try: self.check_consistency() except (smach.InvalidStateError, smach.InvalidTransitionError): smach.logerr("Container consistency check failed.")
def add(label, state, transitions=None, remapping=None): """Add a state to the sequence. Each state added will receive an additional transition from it to the state which is added after it. The transition will follow the outcome specified at construction of this container. @type label: string @param label: The label of the state being added. @param state: An instance of a class implementing the L{State} interface. @param transitions: A dictionary mapping state outcomes to other state labels. If one of these transitions follows the connector outcome specified in the constructor, the provided transition will override the automatically generated connector transition. """ # Get currently opened container self = Sequence._currently_opened_container() if transitions is None: transitions = {} # Perform sequence linking if self._last_added_seq_label is not None: #print self._transitions[self._last_added_seq_label] last_label = self._last_added_seq_label # Check if the connector outcome has been overriden if self._connector_outcome not in self._transitions[last_label]\ or self._transitions[last_label][self._connector_outcome] is None: self._transitions[last_label][self._connector_outcome] = label try: self.check_state_spec(last_label, self._states[last_label], self._transitions[last_label]) except: smach.logerr( "Attempting to construct smach state sequence failed.") #print self._transitions[self._last_added_seq_label] # Store the last added state label self._last_added_seq_label = label return smach.StateMachine.add(label, state, transitions, remapping)
def add(label, state, transitions = None, remapping = None): """Add a state to the sequence. Each state added will receive an additional transition from it to the state which is added after it. The transition will follow the outcome specified at construction of this container. @type label: string @param label: The label of the state being added. @param state: An instance of a class implementing the L{State} interface. @param transitions: A dictionary mapping state outcomes to other state labels. If one of these transitions follows the connector outcome specified in the constructor, the provided transition will override the automatically generated connector transition. """ # Get currently opened container self = Sequence._currently_opened_container() if transitions is None: transitions = {} # Perform sequence linking if self._last_added_seq_label is not None: #print self._transitions[self._last_added_seq_label] last_label = self._last_added_seq_label # Check if the connector outcome has been overriden if self._connector_outcome not in self._transitions[last_label]\ or self._transitions[last_label][self._connector_outcome] is None: self._transitions[last_label][self._connector_outcome] = label try: self.check_state_spec(last_label, self._states[last_label], self._transitions[last_label]) except: smach.logerr("Attempting to construct smach state sequence failed.") #print self._transitions[self._last_added_seq_label] # Store the last added state label self._last_added_seq_label = label return smach.StateMachine.add(label, state, transitions, remapping)
def __init__(self, message): smach.logerr("InvalidUserCodeError: " + message) self.message = message
def __setattr__(self, name, value): if not self.__dict__.has_key('_const__initialized'): return object.__setattr__(self, name, value) smach.logerr("Attempting to set '%s' but this member is read-only." % name) raise TypeError()
def __init__(self,message): smach.logerr("InvalidTransitionError: "+message) self.message = message
def __setitem__(self, key, item): if key not in self._output: smach.logerr("Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s." % (key, self._output)) return self._ud.__setitem__(self._remap(key),item)
def __delattr__(self, name): smach.logerr("Attempting to delete '%s' but this member is read-only." % name) raise TypeError()
def __exit__(self, exc_type, exc_val, exc_tb): if exc_type is None: return self.close() else: if exc_type != smach.InvalidStateError and exc_type != smach.InvalidTransitionError: smach.logerr("Error raised during SMACH container construction: \n" + "\n".join(traceback.format_exception(exc_type, exc_val, exc_tb)))
def execute(self, parent_ud = smach.UserData()): """Overloaded execute method. This starts all the threads. """ # Reset child outcomes self._child_outcomes = {} # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo("Concurrence starting with userdata: \n\t%s" % (str(self.userdata.keys()))) # Call start callbacks self.call_start_cbs() # Create all the threads for (label, state) in self._states.iteritems(): # Initialize child outcomes self._child_outcomes[label] = None self._threads[label] = threading.Thread( name='concurrent_split:'+label, target=self._state_runner, args=(label,)) # Launch threads for thread in self._threads.values(): thread.start() # Wait for done notification self._done_cond.acquire() self._done_cond.wait() self._done_cond.release() # Preempt any running states smach.logdebug("SMACH Concurrence preempting running states.") for label in self._states: if self._child_outcomes[label] == None: self._states[label].request_preempt() # Wait for all states to terminate while not smach.is_shutdown(): if all([o is not None for o in self._child_outcomes.values()]): break self._done_cond.acquire() self._done_cond.wait() self._done_cond.release() # Check for user code exception if self._user_code_exception: self._user_code_exception = False raise smach.InvalidStateError("A concurrent state raised an exception during execution.") # Check for preempt if self.preempt_requested(): # initialized serviced flag children_preempts_serviced = True # Service this preempt if for (label,state) in self._states.iteritems(): if state.preempt_requested(): # Reset the flag children_preempts_serviced = False # Complain smach.logwarn("State '%s' in concurrence did not service preempt." % label) # Recall the preempt if it hasn't been serviced state.recall_preempt() if children_preempts_serviced: smach.loginfo("Concurrence serviced preempt.") self.service_preempt() # Spew some debyg info smach.loginfo("Concurrent Outcomes: "+str(self._child_outcomes)) # Initialize the outcome outcome = self._default_outcome # Determine the outcome from the outcome map smach.logdebug("SMACH Concurrence determining contained state outcomes.") for (container_outcome, outcomes) in self._outcome_map.iteritems(): if all([self._child_outcomes[label] == outcomes[label] for label in outcomes.keys()]): smach.logdebug("Terminating concurrent split with mapped outcome.") outcome = container_outcome # Check outcome callback if self._outcome_cb: try: cb_outcome = self._outcome_cb(copy.copy(self._child_outcomes)) if cb_outcome: if cb_outcome == str(cb_outcome): outcome = cb_outcome else: smach.logerr("Outcome callback returned a non-string '%s', using default outcome '%s'" % (str(cb_outcome), self._default_outcome)) else: smach.logwarn("Outcome callback returned None, using outcome '%s'" % outcome) except: raise smach.InvalidUserCodeError(("Could not execute outcome callback '%s': " % self._outcome_cb)+traceback.format_exc()) # Cleanup self._threads = {} self._child_outcomes = {} # Call termination callbacks self.call_termination_cbs(self._states.keys(), outcome) # Copy output keys self._copy_output_keys(self.userdata, parent_ud) return outcome
def __delattr__(self, name): smach.logerr( "Attempting to delete '%s' but this member is read-only." % name) raise TypeError()
def __init__(self,message): smach.logerr("InvalidConstructionError: "+message) self.message = message
def __getitem__(self,key): if key not in self._states: smach.logerr("Attempting to get state '%s' from StateMachine container. The only available states are: %s" % (key, self._states.keys())) raise KeyError() return self._states[key]
def __init__(self, message): smach.logerr("InvalidConstructionError: " + message) self.message = message
def __init__(self, message): smach.logerr("InvalidTransitionError: " + message) self.message = message
def _update_once(self): """Method that updates the state machine once. This checks if the current state is ready to transition, if so, it requests the outcome of the current state, and then extracts the next state label from the current state's transition dictionary, and then transitions to the next state. """ yield outcome = None transition_target = None last_state_label = self._current_label # Make sure the state exists if self._current_label not in self._states.keys(): raise smach.InvalidStateError("State '%s' does not exist. Available states are: %s" % (str(self._current_label),str(self._states.keys()))) # Check if a preempt was requested before or while the last state was running if self.preempt_requested(): smach.loginfo("Preempt requested on state machine before executing the next state.") # We were preempted if self._preempted_state is not None: # We were preempted while the last state was running if self._preempted_state.preempt_requested(): smach.loginfo("Last state '%s' did not service preempt. Preempting next state '%s' before executing..." % (self._preempted_label, self._current_label)) # The flag was not reset, so we need to keep preempting # (this will reset the current preempt) self._preempt_current_state() else: # The flag was reset, so the container can reset self._preempt_requested = False else: # We were preempted after the last state was running # So we should preempt this state before we execute it self._preempt_current_state() # Execute the state try: self._state_transitioning_lock.release() outcome = yield self._current_state.execute_async( smach.Remapper( self.userdata, self._current_state.get_registered_input_keys(), self._current_state.get_registered_output_keys(), self._remappings[self._current_label])) except smach.InvalidUserCodeError as ex: smach.logerr("State '%s' failed to execute." % self._current_label) raise ex except: raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " % ( self._current_label, self._current_state ) + traceback.format_exc()) finally: self._state_transitioning_lock.acquire() # Check if outcome was a potential outcome for this type of state if outcome not in self._current_state.get_registered_outcomes(): raise smach.InvalidTransitionError("Attempted to return outcome '%s' from state '%s' of type '%s' which only has registered outcomes: %s" % ( str(outcome), str(self._current_label), str(self._current_state), str(self._current_state.get_registered_outcomes()))) # Check if this outcome is actually mapped to any target if outcome not in self._current_transitions: raise smach.InvalidTransitionError("Outcome '%s' of state '%s' is not bound to any transition target. Bound transitions include: %s" % (str(outcome), str(self._current_label), str(self._current_transitions))) # Set the transition target transition_target = self._current_transitions[outcome] # Check if the transition target is a state in this state machine, or an outcome of this state machine if transition_target in self._states: # Set the new state self._set_current_state(transition_target) # Spew some info smach.loginfo("State machine transitioning '%s':'%s'-->'%s'" % (str(last_state_label), str(outcome), str(transition_target))) # Call transition callbacks self.call_transition_cbs() else: # This is a terminal state if self._preempt_requested and self._preempted_state is not None: if not self._current_state.preempt_requested(): self.service_preempt() if transition_target not in self.get_registered_outcomes(): # This is a container outcome that will fall through transition_target = outcome if transition_target in self.get_registered_outcomes(): # The transition target is an outcome of the state machine self._set_current_state(None) # Spew some info smach.loginfo("State machine terminating '%s':'%s':'%s'" % (str(last_state_label), str(outcome), str(transition_target))) # Call termination callbacks self.call_termination_cbs([last_state_label],transition_target) async.returnValue(transition_target) else: raise smach.InvalidTransitionError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." % (str(outcome), str(self._current_label), str(transition_target))) async.returnValue(None)
def __getitem__(self,key): if key != self._state_label: smach.logerr("Attempting to get state '%s' from Iterator container. The only available state is '%s'." % (key, self._state_label)) raise KeyError() return self._state
def __init__(self,message): smach.logerr("InvalidUserCodeError: "+message) self.message = message