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 _state_runner(self,label):
"""Runs the states in parallel threads."""
self.call_transition_cbs()
# Execute child state
try:
self._child_outcomes[label] = self._states[label].execute(smach.Remapper(
self.userdata,
self._states[label].get_registered_input_keys(),
self._states[label].get_registered_output_keys(),
self._remappings[label]))
except:
self._user_code_exception = True
with self._done_cond:
self._done_cond.notify_all()
raise smach.InvalidStateError(("Could not execute child state '%s': " % label)+traceback.format_exc())
# Make sure the child returned an outcome
if self._child_outcomes[label] is None:
raise smach.InvalidStateexception("Concurrent state '%s' returned no outcome on termination." % label)
else:
smach.loginfo("Concurrent state '%s' returned outcome '%s' on termination." % (label, self._child_outcomes[label]))
# Check if all of the states have completed
with self._done_cond:
# initialize preemption flag
preempt_others = False
# Call transition cb's
self.call_transition_cbs()
# Call child termination cb if it's defined
if self._child_termination_cb:
try:
preempt_others = self._child_termination_cb(self._child_outcomes)
except:
raise smach.InvalidUserCodeError("Could not execute child termination callback: "+traceback.format_exc())
# Notify the container to terminate (and preempt other states if neceesary)
if preempt_others or all([o is not None for o in self._child_outcomes.values()]):
self._done_cond.notify_all()
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 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
