def execute(self, parent_ud=smach.UserData()):
# First time in this state machine
if self._current_state is None:
# Spew some info
smach.loginfo(
"State machine starting in initial state '%s' with userdata: \n\t%s"
% (self._current_label, list(self.userdata.keys())))
# Set initial state
self._set_current_state(self._initial_state_label)
# Initialize preempt state
self._preempted_label = None
self._preempted_state = None
# Call start callbacks
self.call_start_cbs()
# Copy input keys
self._copy_input_keys(parent_ud, self.userdata)
container_outcome = self._loopback_name
if self.id is not None:
# only top-level statemachine should loop for outcome
while container_outcome == self._loopback_name and not smach.is_shutdown(
):
# Update the state machine
container_outcome = self._async_execute(parent_ud)
else:
container_outcome = self._async_execute(parent_ud)
if smach.is_shutdown():
container_outcome = 'preempted'
return container_outcome
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 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 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):
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 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 _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 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 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 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 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
