def set_contained_state(label,
state,
loop_outcomes=[],
break_outcomes=[],
final_outcome_map={}):
"""Set the contained state
@type label: string
@param label: The label of the state being added.
@type state: L{smach.State}
@param state: An instance of a class implementing the L{smach.State} interface.
@param loop_outcomes: List of contained state outcomes that should cause
the iterator to continue. If this is empty, all outcomes that are not
in the break_outcomes list will cause the iterator to continue to
iterate. NOTE: loop_outcomes will be overriden by break_outcomes if both
parameters are used.
@param break_outcomes: List of contained state outcomes that should
cause the iterator to break. When the contained state emits an outcome
in this list, the container will terminate and return either that
outcome or the outcome it is mapped to in final_outcome_map. NOTE:
loop_outcomes will be overriden by break_outcomes if both
parameters are used.
@param final_outcome_map: A map from contained state outcomes to container
outcomes. On termination of the iterator (either from finishing or from
a break) this map will be used to translate contained state outcomes to
container outcomes.
Unspecified contained state outcomes will fall through as
container outcomes.
"""
# Get currently opened container
self = Iterator._currently_opened_container()
self._state_label = label
self._state = state
# Get potential state outcomes
state_outcomes = state.get_registered_outcomes()
# Check for loop and break outcomes
if loop_outcomes and break_outcomes:
smach.logwarn(
'Both loop_outcomes and break_outcomes were specified when constructing SMACH iterator container.'
)
if break_outcomes:
self._break_outcomes = break_outcomes
for outcome in state_outcomes:
if outcome not in break_outcomes:
self._loop_outcomes.append(outcome)
else:
self._loop_outcomes = loop_outcomes
for outcome in state_outcomes:
if outcome not in loop_outcomes:
self._break_outcomes.append(outcome)
self._final_outcome_map = final_outcome_map
def set_contained_state(
label,
state,
loop_outcomes = [],
break_outcomes = [],
final_outcome_map = {}):
"""Set the contained state
@type label: string
@param label: The label of the state being added.
@type state: L{smach.State}
@param state: An instance of a class implementing the L{smach.State} interface.
@param loop_outcomes: List of contained state outcomes that should cause
the iterator to continue. If this is empty, all outcomes that are not
in the break_outcomes list will cause the iterator to continue to
iterate. NOTE: loop_outcomes will be overriden by break_outcomes if both
parameters are used.
@param break_outcomes: List of contained state outcomes that should
cause the iterator to break. When the contained state emits an outcome
in this list, the container will terminate and return either that
outcome or the outcome it is mapped to in final_outcome_map. NOTE:
loop_outcomes will be overriden by break_outcomes if both
parameters are used.
@param final_outcome_map: A map from contained state outcomes to container
outcomes. On termination of the iterator (either from finishing or from
a break) this map will be used to translate contained state outcomes to
container outcomes.
Unspecified contained state outcomes will fall through as
container outcomes.
"""
# Get currently opened container
self = Iterator._currently_opened_container()
self._state_label = label
self._state = state
# Get potential state outcomes
state_outcomes = state.get_registered_outcomes()
# Check for loop and break outcomes
if loop_outcomes and break_outcomes:
smach.logwarn('Both loop_outcomes and break_outcomes were specified when constructing SMACH iterator container.')
if break_outcomes:
self._break_outcomes = break_outcomes
for outcome in state_outcomes:
if outcome not in break_outcomes:
self._loop_outcomes.append(outcome)
else:
self._loop_outcomes = loop_outcomes
for outcome in state_outcomes:
if outcome not in loop_outcomes:
self._break_outcomes.append(outcome)
self._final_outcome_map = final_outcome_map
def _copy_output_keys(self, ud, parent_ud):
if parent_ud is not None:
output_keys = self.get_registered_output_keys()
for ok in output_keys:
try:
parent_ud[ok] = ud[ok]
except KeyError:
smach.logwarn("Attempting to copy output key '%s', but this key does not exist.")
def _copy_input_keys(self, parent_ud, ud):
if parent_ud is not None:
input_keys = self.get_registered_input_keys()
for ik in input_keys:
try:
ud[ik] = parent_ud[ik]
except KeyError:
smach.logwarn("Attempting to copy input key '%s', but this key does not exist.")
def _copy_output_keys(self, ud, parent_ud):
if parent_ud is not None:
output_keys = self.get_registered_output_keys()
for ok in output_keys:
try:
parent_ud[ok] = ud[ok]
except KeyError:
smach.logwarn(
"Attempting to copy output key '%s', but this key does not exist."
)
def _copy_input_keys(self, parent_ud, ud):
if parent_ud is not None:
input_keys = self.get_registered_input_keys()
for ik in input_keys:
try:
ud[ik] = parent_ud[ik]
except KeyError:
smach.logwarn(
"Attempting to copy input key '%s', but this key does not exist."
)
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 set_initial_state(self, initial_states, userdata):
# Check initial state
if len(initial_states) > 1:
smach.logwarn("Attempting to set initial state to include more than one state, but Iterator container can only have one initial state." % (self._state_label))
if len(initial_states) > 0:
if initial_states[0] != self._state_label:
smach.logwarn("Attempting to set state '%s' as initial state in Iterator container. The only available state is '%s'." % (initial_states[0], self._state_label))
raise KeyError()
# Set local userdata
self.userdata.update(userdata)
def set_initial_state(self, initial_states, userdata):
# Check initial state
if len(initial_states) > 1:
smach.logwarn(
"Attempting to set initial state to include more than one state, but Iterator container can only have one initial state."
% (self._state_label))
if len(initial_states) > 0:
if initial_states[0] != self._state_label:
smach.logwarn(
"Attempting to set state '%s' as initial state in Iterator container. The only available state is '%s'."
% (initial_states[0], self._state_label))
raise KeyError()
# Set local userdata
self.userdata.update(userdata)
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
