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)
