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 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 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 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