def get_const(obj): """Get a const reference to an object if it has "user-defined" attributes.""" if hasattr(obj,'__dict__'): smach.logdebug("Making const '%s'" % str(obj)) return Const(obj) else: return obj
def get_const(obj): """Get a const reference to an object if it has "user-defined" attributes.""" if hasattr(obj, '__dict__'): smach.logdebug("Making const '%s'" % str(obj)) return Const(obj) else: return obj
def set_initial_state(self, initial_states, userdata=smach.UserData()): smach.logdebug("Setting initial state to "+str(initial_states)) if len(initial_states) > 1: smach.logwarn("Attempting to set initial state to include more than one state, but the StateMachine container can only have one initial state.") # Set the initial state label if len(initial_states) > 0: self._initial_state_label = initial_states[0] # Set local userdata self.userdata.update(userdata)
def __getitem__(self, name): smach.logdebug("Getting '%s' from const wrapper." % name) attr = self._obj[name] return get_const(attr)
def __getattr__(self, name): smach.logdebug("Getting '%s' from const wrapper." % name) attr = getattr(self._obj, name) return get_const(attr)
def __init__(self, obj): smach.logdebug("Making const '%s'" % str(obj)) self._obj = obj self.__initialized = True
def add(label, state, transitions = None, remapping = None): """Add a state to the opened state machine. @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 or container outcomes. @param remapping: A dictrionary mapping local userdata keys to userdata keys in the container. """ # Get currently opened container self = StateMachine._currently_opened_container() smach.logdebug('Adding state (%s, %s, %s)' % (label, str(state), str(transitions))) # Set initial state if it is still unset if self._initial_state_label is None: self._initial_state_label = label if transitions is None: transitions = {} if remapping is None: remapping = {} # Add group transitions to this new state, if they exist """ if 'transitions' in smach.Container._context_kwargs: for outcome, target in smach.Container._context_kwargs['transitions'].iteritems(): if outcome not in transitions: transitions[outcome] = target """ # Check the state specification self.check_state_spec(label, state, transitions) # Check if th label already exists if label in self._states: raise smach.InvalidStateError( 'Attempting to add state with label "'+label+'" to state machine, but this label is already being used.') # Debug info smach.logdebug("Adding state '"+str(label)+"' to the state machine.") # Create implicit terminal transitions, and combine them with the explicit transitions registered_outcomes = state.get_registered_outcomes() # Get a list of the unbound transitions missing_transitions = dict([(o,None) for o in registered_outcomes if o not in transitions.keys()]) transitions.update(missing_transitions) smach.logdebug("State '%s' is missing transitions: %s" % (label,str(missing_transitions))) # Add state and transitions to the dictionary self._states[label] = state self._transitions[label] = transitions self._remappings[label] = remapping smach.logdebug("TRANSITIONS FOR %s: %s" % (label, str(self._transitions[label]))) # Add transition to this state if connected outcome is defined if len(self._connector_outcomes) > 0 and self._last_added_label is not None: for connector_outcome in self._connector_outcomes: self._transitions[self._last_added_label][connector_outcome] = label # Reset connector outcomes and last added label self._connector_outcomes = [] self._last_added_label = None return state
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 __init__(self, obj): smach.logdebug("Making const '%s'" % str(obj)) self._obj = obj self.__initialized = True
def __getitem__(self, name): smach.logdebug("Getting '%s' from const wrapper." % name) attr = self._obj[name] return get_const(attr)
def __getattr__(self, name): smach.logdebug("Getting '%s' from const wrapper." % name) attr = getattr(self._obj,name) return get_const(attr)