def call_update_tree_view_cb(self):
if (self._tree_view_cb is not None):
try:
self._tree_view_cb()
except:
smach.logerr("Could not execute treeview callback: " +
traceback.format_exc())
def __getitem__(self, key):
if key not in self._states:
smach.logerr(
"Attempting to get state '%s' from StateMachine container. The only available states are: %s"
% (key, str(list(self._states.keys()))))
raise KeyError()
return self._states[key]
def __getitem__(self, key):
if key != self._state_label:
smach.logerr(
"Attempting to get state '%s' from Iterator container. The only available state is '%s'."
% (key, self._state_label))
raise KeyError()
return self._state
def __setitem__(self, key, item):
if key not in self._output:
smach.logerr(
"Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s."
% (key, self._output))
return
self._ud.__setitem__(self._remap(key), item)
def __setattr__(self, name, value):
if name[0] == '_' or not self.__dict__.has_key('_Remapper__initialized'):
return object.__setattr__(self, name, value)
if name not in self._output:
smach.logerr("Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s." % (name, self._output))
return None
setattr(self._ud, self._remap(name), value)
def __setattr__(self, name, value):
if name[0] == '_' or '_Remapper__initialized' not in self.__dict__:
return object.__setattr__(self, name, value)
if name not in self._output:
smach.logerr("Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s." % (name, self._output))
return None
setattr(self._ud, self._remap(name), value)
def _async_execute(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.")
return None
# Set running flag
self._is_running = True
# Initialize container outcome
container_outcome = None
# Step through state machine
if self._is_running and not smach.is_shutdown():
# Update the state machine
container_outcome = self._update_once()
if self._do_rate_sleep and self._current_state is not None and not isinstance(
self._current_state, OperatableStateMachine):
try:
# sleep with the rate of the state and update container rate accordingly
self._rate = self._current_state._rate
self._rate.sleep()
except ROSInterruptException:
rospy.logwarn('Interrupted rate sleep.')
if container_outcome is not None and container_outcome != self._loopback_name:
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
else:
container_outcome = self._loopback_name
# provide explicit sync as back-up functionality
# should be used only if there is no other choice
# since it requires additional 8 byte + header update bandwith and time to restart mirror
if self._inner_sync_request and self._get_deep_state() is not None:
self._inner_sync_request = False
if self.id is None:
self._parent._inner_sync_request = True
else:
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(
self._get_deep_state()._get_path())
self._pub.publish('flexbe/mirror/sync', msg)
# We're no longer running
self._is_running = False
return container_outcome
def __getitem__(self, key):
if key not in self._states:
smach.logerr(
"Attempting to get state '%s' from StateMachine container. The only available states are: %s"
% (key, self._states.keys())
)
raise KeyError()
return self._states[key]
def __setitem__(self, key, item):
if key not in self._output:
smach.logerr(
"Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s."
% (key, self._output)
)
return
self._ud.__setitem__(self._remap(key), item)
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 = next(it)
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():
self.service_preempt()
outcome = 'preempted'
break
if outcome in self._break_outcomes\
or (len(self._loop_outcomes) > 0 and outcome not in self._loop_outcomes):
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 = 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.")
return 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, list(self.userdata.keys())))
if self._preempt_requested:
smach.logwarn("Preempt on State machine requested before even executing initial state. This could be a bug. Did last execution not service preemption?")
# 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 = self._update_once()
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
# We're no longer running
self._is_running = False
if self._preempt_requested:
self.service_preempt()
return container_outcome
def __exit__(self, exc_type, exc_val, exc_tb):
if exc_type is None:
return self.close()
else:
if exc_type != smach.InvalidStateError and exc_type != smach.InvalidTransitionError:
smach.logerr(
"Error raised during SMACH container construction: \n" +
"\n".join(
traceback.format_exception(exc_type, exc_val, exc_tb)))
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 = next(it)
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 extract(self, keys, remapping):
ud = UserData()
reverse_remapping = {remapping[k]: k for k in remapping}
if len(reverse_remapping) != len(remapping):
smach.logerr("SMACH userdata remapping is not one-to-one: " + str(remapping))
for k in keys:
rmk = k
if k in reverse_remapping:
rmk = reverse_remapping[k]
ud[rmk] = copy.copy(self[k])
return ud
def extract(self, keys, remapping):
ud = UserData()
reverse_remapping = dict([(v,k) for (k,v) in remapping.iteritems()])
if len(reverse_remapping) != len(remapping):
smach.logerr("SMACH userdata remapping is not one-to-one: " + str(remapping))
for k in keys:
rmk = k
if k in reverse_remapping:
rmk = reverse_remapping[k]
ud[rmk] = copy.copy(self[k])
return ud
def call_transition_cbs(self):
"""Calls the registered transition callbacks.
Callback functions are called with two arguments in addition to any
user-supplied arguments:
- userdata
- a list of active states
"""
try:
for (cb,args) in self._transition_cbs:
cb(self.userdata, self.get_active_states(), *args)
except:
smach.logerr("Could not execute transition callback: "+traceback.format_exc())
def execute(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.")
return 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 = self._update_once()
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
# We're no longer running
self._is_running = False
return container_outcome
def call_transition_cbs(self):
"""Calls the registered transition callbacks.
Callback functions are called with two arguments in addition to any
user-supplied arguments:
- userdata
- a list of active states
"""
try:
for (cb, args) in self._transition_cbs:
cb(self.userdata, self.get_active_states(), *args)
except:
smach.logerr("Could not execute transition callback: " +
traceback.format_exc())
def call_termination_cbs(self, terminal_states, outcome):
"""Calls the registered termination callbacks.
Callback functions are called with three arguments in addition to any
user-supplied arguments:
- userdata
- a list of terminal states
- the outcome of this container
"""
try:
for (cb,args) in self._termination_cbs:
cb(self.userdata, terminal_states, outcome, *args)
except:
smach.logerr("Could not execute termination callback: "+traceback.format_exc())
def call_termination_cbs(self, terminal_states, outcome):
"""Calls the registered termination callbacks.
Callback functions are called with three arguments in addition to any
user-supplied arguments:
- userdata
- a list of terminal states
- the outcome of this container
"""
try:
for (cb, args) in self._termination_cbs:
cb(self.userdata, terminal_states, outcome, *args)
except:
smach.logerr("Could not execute termination callback: " +
traceback.format_exc())
def __getattr__(self, name):
"""Override getattr to be thread safe."""
if name[0] == '_':
return object.__getattr__(self, name)
if not name in self._locks.keys():
self._locks[name] = threading.Lock()
try:
with self._locks[name]:
temp = self._data[name]
except:
smach.logerr("Userdata key '%s' not available. Available keys are: %s" % (name, str(list(self._data.keys()))))
raise KeyError()
return temp
def __getattr__(self, name):
"""Overload getattr to be thread safe."""
if name[0] == '_':
return object.__getattr__(self, name)
if not name in self._locks.keys():
self._locks[name] = threading.Lock()
try:
with self._locks[name]:
temp = self._data[name]
except:
smach.logerr("Userdata key '%s' not available. Available keys are: %s" % (name, self._data.keys()))
raise KeyError()
return temp
def close(self):
"""Close the container."""
# Make sure this container is the currently open container
if len(Container._construction_stack) > 0:
if self != Container._construction_stack[-1]:
raise smach.InvalidStateError('Attempting to close a container that is not currently open.')
# Pop this container off the construction stack
Container._construction_stack.pop()
Container._construction_lock.release()
# Check consistency of container, post-construction
try:
self.check_consistency()
except (smach.InvalidStateError, smach.InvalidTransitionError):
smach.logerr("Container consistency check failed.")
def close(self):
"""Close the container task."""
# Make sure this container is the currently open container
if len(ContainerMixin._construction_stack) > 0:
if self != ContainerMixin._construction_stack[-1]:
raise smach.InvalidStateError('Attempting to close a container task that is not currently open.')
# Pop this container off the construction stack
ContainerMixin._construction_stack.pop()
ContainerMixin._construction_lock.release()
# Check consistency of container, post-construction
try:
self.check_consistency()
except (smach.InvalidStateError, smach.InvalidTransitionError):
smach.logerr("Container task consistency check failed.")
def _preempt_current_state(self):
"""Preempt the current state (might not be executing yet).
This also resets the preempt flag on a state that had previously received the preempt, but not serviced it."""
if self._preempted_state != self._current_state:
if self._preempted_state is not None:
# Reset the previously preempted state (that has now terminated)
self._preempted_state.recall_preempt()
# Store the label of the currently active state
self._preempted_state = self._current_state
self._preempted_label = self._current_label
# Request the currently active state to preempt
try:
self._preempted_state.request_preempt()
except:
smach.logerr("Failed to preempt contained state '%s': %s" % (self._preempted_label, traceback.format_exc()))
def _preempt_current_state(self):
"""Preempt the current state (might not be executing yet).
This also resets the preempt flag on a state that had previously received the preempt, but not serviced it."""
if self._preempted_state != self._current_state:
if self._preempted_state is not None:
# Reset the previously preempted state (that has now terminated)
self._preempted_state.recall_preempt()
# Store the label of the currently active state
self._preempted_state = self._current_state
self._preempted_label = self._current_label
# Request the currently active state to preempt
try:
self._preempted_state.request_preempt()
except:
smach.logerr("Failed to preempt contained state '%s': %s" % (self._preempted_label, traceback.format_exc()))
def add(label, state, transitions=None, remapping=None):
"""Add a state to the sequence.
Each state added will receive an additional transition from it to the
state which is added after it. The transition will follow the outcome
specified at construction of this container.
@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. If one of these transitions follows the connector outcome
specified in the constructor, the provided transition will override
the automatically generated connector transition.
"""
# Get currently opened container
self = Sequence._currently_opened_container()
if transitions is None:
transitions = {}
# Perform sequence linking
if self._last_added_seq_label is not None:
#print self._transitions[self._last_added_seq_label]
last_label = self._last_added_seq_label
# Check if the connector outcome has been overriden
if self._connector_outcome not in self._transitions[last_label]\
or self._transitions[last_label][self._connector_outcome] is None:
self._transitions[last_label][self._connector_outcome] = label
try:
self.check_state_spec(last_label, self._states[last_label],
self._transitions[last_label])
except:
smach.logerr(
"Attempting to construct smach state sequence failed.")
#print self._transitions[self._last_added_seq_label]
# Store the last added state label
self._last_added_seq_label = label
return smach.StateMachine.add(label, state, transitions, remapping)
def _async_execute(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.")
return None
# Set running flag
self._is_running = True
# Initialize container outcome
container_outcome = None
# Step through state machine
if self._is_running and not smach.is_shutdown():
# Update the state machine
container_outcome = self._update_once()
if self._current_state is not None:
try:
self._current_state._rate.sleep()
except ROSInterruptException:
rospy.logwarn('Interrupted rate sleep.')
if container_outcome is not None and container_outcome != self._loopback_name:
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
else:
container_outcome = self._loopback_name
# We're no longer running
self._is_running = False
return container_outcome
def add(label, state, transitions = None, remapping = None):
"""Add a state to the sequence.
Each state added will receive an additional transition from it to the
state which is added after it. The transition will follow the outcome
specified at construction of this container.
@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. If one of these transitions follows the connector outcome
specified in the constructor, the provided transition will override
the automatically generated connector transition.
"""
# Get currently opened container
self = Sequence._currently_opened_container()
if transitions is None:
transitions = {}
# Perform sequence linking
if self._last_added_seq_label is not None:
#print self._transitions[self._last_added_seq_label]
last_label = self._last_added_seq_label
# Check if the connector outcome has been overriden
if self._connector_outcome not in self._transitions[last_label]\
or self._transitions[last_label][self._connector_outcome] is None:
self._transitions[last_label][self._connector_outcome] = label
try:
self.check_state_spec(last_label, self._states[last_label], self._transitions[last_label])
except:
smach.logerr("Attempting to construct smach state sequence failed.")
#print self._transitions[self._last_added_seq_label]
# Store the last added state label
self._last_added_seq_label = label
return smach.StateMachine.add(label, state, transitions, remapping)
def _execute_state(self, state, force_exit=False):
result = None
try:
ud = smach.Remapper(
self.userdata,
state.get_registered_input_keys(),
state.get_registered_output_keys(),
self._remappings[state.name])
if force_exit:
state.on_exit(ud)
else:
result = state.execute(ud)
#print 'execute %s --> %s' % (state.name, self._returned_outcomes[state.name])
except smach.InvalidUserCodeError as ex:
smach.logerr("State '%s' failed to execute." % state.name)
raise ex
except:
raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
(state.name, state)
+ traceback.format_exc())
return result
def __init__(self, message):
smach.logerr(self.__class__.__name__ + ": " + message)
Exception.__init__(self, message)
def execute(self, parent_ud=smach.UserData()):
"""Overridden execute method.
This starts all the threads.
"""
# Clear the ready event
self._ready_event.clear()
# 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(list(self.userdata.keys()))))
# Call start callbacks
self.call_start_cbs()
# Create all the threads
for (label, state) in ((k, self._states[k]) for k in self._states):
# 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()
# Notify all threads ready to go
self._ready_event.set()
# Wait for a done notification from a thread
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([not t.isAlive() for t in self._threads.values()]):
break
self._done_cond.acquire()
self._done_cond.wait(0.1)
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 ((k, self._states[k]) for k in self._states):
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 ((k, self._outcome_map[k]) for k in self._outcome_map):
if all([self._child_outcomes[label] == outcomes[label] for label in outcomes]):
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(list(self._states.keys()), outcome)
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
return outcome
def execute(self, parent_ud=smach.UserData()):
"""Overridden execute method.
This starts all the threads.
"""
# Clear the ready event
self._ready_event.clear()
# Reset child outcomes
self._child_outcomes = {}
# Copy input keys
self._copy_input_keys(parent_ud, self.userdata)
## Copy the datamodel's value into the userData
for data in self._datamodel:
if (self._datamodel[data] != ""):
self.userdata[data] = self._datamodel[data]
## Do the <onentry>
if (self._onEntry is not None):
try:
self._onEntry.execute(self.userdata)
except Exception as ex:
rospy.logerr('%s::onEntry::execute() raised | %s' %
(self.__class__.__name__, str(ex)))
return "preempt"
# Spew some info
smach.loginfo("Concurrence starting with userdata: \n\t%s" %
(str(list(self.userdata.keys()))))
# Call start callbacks
self.call_start_cbs()
# Create all the threads
for (label, state) in ((k, self._states[k]) for k in self._states):
# 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()
# Notify all threads ready to go
self._ready_event.set()
# Wait for a done notification from a thread
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([not t.isAlive() for t in self._threads.values()]):
break
self._done_cond.acquire()
self._done_cond.wait(0.1)
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 ((k, self._states[k]) for k in self._states):
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 ((k, self._outcome_map[k])
for k in self._outcome_map):
if all([
self._child_outcomes[label] == outcomes[label]
for label in outcomes
]):
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(list(self._states.keys()), outcome)
## Do the <onexit>
if (self._onExit is not None):
try:
outcome = self._onExit.execute(self.userdata, outcome)
except Exception as ex:
rospy.logerr('%s::onExit::execute() raised | %s' %
(self.__class__.__name__, str(ex)))
return "preempt"
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
return outcome
def __init__(self, message):
smach.logerr("InvalidConstructionError: " + message)
self.message = message
def __init__(self,message):
smach.logerr("InvalidConstructionError: "+message)
self.message = message
def __init__(self,message):
smach.logerr("InvalidUserCodeError: "+message)
self.message = message
def __init__(self, message):
smach.logerr("InvalidTransitionError: " + message)
self.message = message
def __init__(self,message):
smach.logerr("InvalidTransitionError: "+message)
self.message = message
def __exit__(self, exc_type, exc_val, exc_tb):
if exc_type is None:
return self.close()
else:
if exc_type != smach.InvalidStateError and exc_type != smach.InvalidTransitionError:
smach.logerr("Error raised during SMACH container construction: \n" + "\n".join(traceback.format_exception(exc_type, exc_val, exc_tb)))
def __getitem__(self,key):
if key != self._state_label:
smach.logerr("Attempting to get state '%s' from Iterator container. The only available state is '%s'." % (key, self._state_label))
raise KeyError()
return self._state
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.
"""
outcome = None
transition_target = None
last_state_label = self._current_label
# Make sure the state exists
if self._current_label not in self._states:
raise smach.InvalidStateError("State '%s' does not exist. Available states are: %s" %
(self._current_label, list(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
self._preempted_state = None
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 = self._current_state.execute(
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" %
(outcome,
self._current_label,
self._current_state,
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 (isinstance(transition_target, list) and set(transition_target).intersection(self._states)) or (isinstance(transition_target, str) and 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'" %
(last_state_label, outcome, transition_target))
if type(self._states[transition_target] == smach.StateMachineRL):
self._states[transition_target].state_label = transition_target
if 'reward' in self.userdata:
reward = self.userdata['reward']
self.publisher.update_rl(last_state_label, -reward, last_state_label, self.num)
# Call transition callbacks
self.call_transition_cbs()
else:
# This is a terminal state
self.publisher.update_rl('END', 0, 'END', self.num)
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'" %
(last_state_label, outcome, transition_target))
# Call termination callbacks
self.call_termination_cbs([last_state_label],transition_target)
return 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." %
(outcome, self._current_label, transition_target))
return None
def __setattr__(self, name, value):
if not self.__dict__.has_key('_const__initialized'):
return object.__setattr__(self, name, value)
smach.logerr("Attempting to set '%s' but this member is read-only." % name)
raise TypeError()
def __delattr__(self, name):
smach.logerr("Attempting to delete '%s' but this member is read-only." % name)
raise TypeError()
def execute(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.")
return 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, list(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
try:
container_outcome = self._update_once()
except Exception as ex:
# Save the coverage information
if 'SSCOV_FILE' in os.environ:
covbasename = os.environ['SSCOV_FILE']
else:
covbasename = '.scov'
# Determine if suffixes are required
if StateMachine.cov_instance_counter == 1:
covsuffix = ''
else:
covsuffix = '_' + str(self.cov_id)
with open(covbasename + covsuffix, 'w') as covfile:
pickle.dump(self.cov_states, covfile)
with open('.ERRORS_' + covbasename + covsuffix, 'w') as ef:
ef.write(str(ex))
raise
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
# We're no longer running
self._is_running = False
# Save the coverage information
if 'SSCOV_FILE' in os.environ:
covbasename = os.environ['SSCOV_FILE']
else:
covbasename = '.scov'
# Determine if suffixes are required
if StateMachine.cov_instance_counter == 1:
covsuffix = ''
else:
covsuffix = '_' + str(self.cov_id)
with open(covbasename + covsuffix, 'w') as covfile:
pickle.dump(self.cov_states, covfile)
return container_outcome
def __delattr__(self, name):
smach.logerr("Attempting to delete '%s' but this member is read-only." % name)
raise TypeError()
def __setattr__(self, name, value):
if '_const__initialized' not in self.__dict__:
return object.__setattr__(self, name, value)
smach.logerr("Attempting to set '%s' but this member is read-only." % name)
raise TypeError()
def execute(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.")
return 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, list(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
try:
container_outcome = self._update_once()
except Exception as ex:
# Save the coverage information
if 'SSCOV_FILE' in os.environ:
covbasename = os.environ['SSCOV_FILE']
else:
covbasename = '.scov'
# Determine if suffixes are required
if StateMachine.cov_instance_counter == 1:
covsuffix = ''
else:
covsuffix = '_' + str(self.cov_id)
with open(covbasename + covsuffix, 'w') as covfile:
pickle.dump(self.cov_states, covfile)
with open('.ERRORS_' + covbasename + covsuffix, 'w') as ef:
ef.write(str(ex))
raise
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
# We're no longer running
self._is_running = False
# Save the coverage information
if 'SSCOV_FILE' in os.environ:
covbasename = os.environ['SSCOV_FILE']
else:
covbasename = '.scov'
# Determine if suffixes are required
if StateMachine.cov_instance_counter == 1:
covsuffix = ''
else:
covsuffix = '_' + str(self.cov_id)
with open(covbasename + covsuffix, 'w') as covfile:
pickle.dump(self.cov_states, covfile)
return container_outcome
def _update_once(self):
#print 'update'
# Check if a preempt was requested before or while the last state was running
if self.preempt_requested() or PreemptableState.preempt:
#if self._preempted_state is not None:
# if self._preempted_state.preempt_requested():
# self._preempt_current_state()
# else:
# self._preempt_requested = False
# self._preempted_state = None
#else:
# self._preempt_current_state()
return self._preempted_name
#self._state_transitioning_lock.release()
for state in self._ordered_states:
if state.name in self._returned_outcomes.keys() and self._returned_outcomes[state.name] != self._loopback_name:
continue
if PriorityContainer.active_container is not None and not PriorityContainer.active_container.startswith(state._get_path()):
if isinstance(state, EventState):
state._notify_skipped()
elif state._get_deep_state() is not None:
state._get_deep_state()._notify_skipped()
continue
try:
ud = smach.Remapper(
self.userdata,
state.get_registered_input_keys(),
state.get_registered_output_keys(),
self._remappings[state.name])
self._returned_outcomes[state.name] = state.execute(ud)
#print 'execute %s --> %s' % (state.name, self._returned_outcomes[state.name])
except smach.InvalidUserCodeError as ex:
smach.logerr("State '%s' failed to execute." % state.name)
raise ex
except:
raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
(state.name, state)
+ traceback.format_exc())
#self._state_transitioning_lock.acquire()
# Determine outcome
outcome = self._loopback_name
for item in self._conditions:
(oc, cond) = item
if all(self._returned_outcomes.has_key(sn) and self._returned_outcomes[sn] == o for sn,o in cond):
outcome = oc
break
# preempt (?)
if outcome == self._loopback_name:
return None
if outcome in self.get_registered_outcomes():
# Call termination callbacks
self.call_termination_cbs([s.name for s in self._ordered_states],outcome)
self._returned_outcomes = dict()
return outcome
else:
raise smach.InvalidTransitionError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." %
(outcome, self.name, outcome))
def execute(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.")
return 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, list(self.userdata.keys())))
# Call start callbacks
self.call_start_cbs()
# Initialize container outcome
container_outcome = None
## Copy the datamodel's value into the userData
for data in self._datamodel:
self.userdata[data] = self._datamodel[data]
## Do the <onentry>
if (self._onEntry is not None):
try:
self._onEntry.execute(self.userdata)
except Exception as ex:
rospy.logerr('%s::onEntry::execute() raised | %s' %
(self.__class__.__name__, str(ex)))
return "preempt"
# Step through state machine
while container_outcome is None and self._is_running and not smach.is_shutdown(
):
# Update the state machine
if (self._pause):
self._tree_view_pause_state(self._current_label)
cpt = 30
while (self._pause):
if (cpt >= 30):
cpt = 0
rospy.logwarn(
"[SSM] : Smart State Machine is paused")
rospy.logwarn("[SSM] : Next state executed is : " +
self._current_label)
cpt = cpt + 1
rospy.sleep(0.1)
if (self._preempt_pause):
self._tree_view_disable_state(self._current_label)
return "preempt"
container_outcome = self._update_once()
## Do the <onexit>
if (self._onExit is not None):
try:
container_outcome = self._onExit.execute(
self.userdata, container_outcome)
except Exception as ex:
rospy.logerr('%s::onExit::execute() raised | %s' %
(self.__class__.__name__, str(ex)))
return "preempt"
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
# We're no longer running
self._is_running = False
return container_outcome
def __init__(self, message):
smach.logerr(self.__class__.__name__ + ": " + message)
Exception.__init__(self, message)
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.
"""
outcome = None
transition_target = None
last_state_label = self._current_label
# Make sure the state exists
if self._current_label not in self._states:
raise smach.InvalidStateError(
"State '%s' does not exist. Available states are: %s" %
(self._current_label, list(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
self._preempted_state = None
else:
# We were preempted after the last state was running
# So we sho if(self._preempt_requested):uld preempt this state before we execute it
self._preempt_current_state()
# Execute the state
self._tree_view_enable_state(self._current_label)
try:
self._state_transitioning_lock.release()
outcome = self._current_state.execute(
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()
self._tree_view_disable_state(self._current_label)
# 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' preempt_requestedfrom state '%s' of"
" type '%s' which only has registered outcomes: %s" %
(outcome, self._current_label, self._current_state,
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'" %
(last_state_label, outcome, 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'" %
(last_state_label, outcome, transition_target))
# Call termination callbacks
self.call_termination_cbs([last_state_label],
transition_target)
return 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."
% (outcome, self._current_label, transition_target))
return None
def __init__(self, message):
smach.logerr("InvalidUserCodeError: " + message)
self.message = message