def check_consistency(self):
for (co,cso) in ((k,self._outcome_map[k]) for k in self._outcome_map):
for state_label,outcome in ((k,cso[k]) for k in cso):
if outcome not in self._states[state_label].get_registered_outcomes():
raise smach.InvalidTransitionError(
'Outcome map in SMACH Concurrence references a state outcome that does not exist. Requested state outcome: \'%s\', but state \'%s\' only has outcomes %s' %
(outcome, state_label, str(self._states[state_label].get_registered_outcomes())))
def execute(self, parent_ud=smach.UserData()):
with self._condition:
if self._root is None:
raise smach.InvalidTransitionError("BehaviorTreeContainer has no root task")
self._result = None
self._current_exec_root = self._initial_task
# Copy input keys
self._copy_input_keys(parent_ud, self.userdata)
result = self._current_exec_root.try_immediate(self.userdata)
if result == Task.DEFERRED:
self._current_exec_root.execute_deferred(self._root_termination_cb, self.userdata)
while self._current_exec_root is not None:
# Releases condition mutex during wait
self._condition.wait(0.1)
result = self._result
else:
self._current_exec_root = None
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
return self._outcome_from_result(result)
def check_consistency(self):
"""Check the entire state machine for consistency.
This asserts that all transition targets are states that are in the
state machine. If this fails, it raises an L{InvalidTransitionError}
with relevant information.
"""
# Construct a set of available states
available_states = set(
list(self._states.keys()) + list(self.get_registered_outcomes()))
# Grab the registered outcomes for the state machine
registered_sm_outcomes = self.get_registered_outcomes()
# Hopefully this string stays empty
errors = ""
# Check initial_state_label
if self._initial_state_label is None:
errors = errors + "\n\tNo initial state set."
elif self._initial_state_label not in self._states:
errors = errors + "\n\tInitial state label: '" + str(
self._initial_state_label) + "' is not in the state machine."
# Generate state specifications
state_specs = [(label, self._states[label], self._transitions[label])
for label in self._states]
# Iterate over all states
for label, state, transitions in state_specs:
# Check that all potential outcomes are registered in this state
transition_states = set(
[s for s in transitions.values() if s is not None and s != ''])
# Generate a list of missing states
missing_states = transition_states.difference(available_states)
# Check number of missing states
if len(missing_states) > 0:
errors = (errors + "\n\tState '" + str(label) +
"' references unknown states: " +
str(list(missing_states)))
# Check terminal outcomes for this state
terminal_outcomes = set([
o for (o, s) in ((k, transitions[k]) for k in transitions)
if s is None or s == ''
])
# Terminal outcomes should be in the registered outcomes of this state machine
missing_outcomes = terminal_outcomes.difference(
registered_sm_outcomes)
# Check number of missing outcomes
if len(missing_outcomes) > 0:
errors = (errors + "\n\tState '" + str(label) +
"' references unregistered outcomes: " +
str(list(missing_outcomes)))
# Check errors
if len(errors) > 0:
raise smach.InvalidTransitionError(
"State machine failed consistency check: " + errors +
"\n\n\tAvailable states: " + str(list(available_states)))
def _update_once(self):
# Check if a preempt was requested before or while the last state was running
if self.preempt_requested() or PreemptableState.preempt:
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()) \
and not state._get_path().startswith(PriorityContainer.active_container):
if isinstance(state, EventState):
state._notify_skipped()
elif state._get_deep_state() is not None:
state._get_deep_state()._notify_skipped()
continue
self._returned_outcomes[state.name] = self._execute_state(state)
#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.on_exit(
self.userdata,
states=filter(
lambda s: s.name not in self._returned_outcomes.keys() or
self._returned_outcomes[s.name] == self._loopback_name,
self._ordered_states))
self._returned_outcomes = dict()
# right now, going out of a cc may break sync
# thus, as a quick fix, explicitly sync again
self._parent._inner_sync_request = True
self._current_state = None
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 check_state_spec(self, label, state, transitions):
"""Validate full state specification (label, state, and transitions).
This checks to make sure the required variables are in the state spec,
as well as verifies that all outcomes referenced in the transitions
are registered as valid outcomes in the state object. If a state
specification fails validation, a L{smach.InvalidStateError} is
thrown.
"""
# Make sure all transitions are from registered outcomes of this state
registered_outcomes = state.get_registered_outcomes()
for outcome in transitions:
if outcome not in registered_outcomes:
raise smach.InvalidTransitionError("Specified outcome '"+outcome+"' on state '"+label+"', which only has available registered outcomes: "+str(registered_outcomes))
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 _update_once(self):
# Check if a preempt was requested before or while the last state was running
if self.preempt_requested() or PreemptableState.preempt:
return self._preempted_name
#self._state_transitioning_lock.release()
sleep_dur = None
for state in self._ordered_states:
if state.name in list(self._returned_outcomes.keys(
)) and self._returned_outcomes[state.name] != self._loopback_name:
continue
if (PriorityContainer.active_container is not None
and not all(a == s for a, s in zip(
PriorityContainer.active_container.split('/'),
state._get_path().split('/')))):
if isinstance(state, EventState):
state._notify_skipped()
elif state._get_deep_state() is not None:
state._get_deep_state()._notify_skipped()
continue
state_sleep_dur = state._rate.remaining().to_sec()
if state_sleep_dur <= 0:
sleep_dur = 0
self._returned_outcomes[state.name] = self._execute_state(
state)
# check again in case the sleep has already been handled by the child
if state._rate.remaining().to_sec() < 0:
# this sleep returns immediately since sleep duration is negative,
# but is required here to reset the sleep time after executing
state._rate.sleep()
else:
if sleep_dur is None:
sleep_dur = state_sleep_dur
else:
sleep_dur = min(sleep_dur, state_sleep_dur)
if sleep_dur > 0:
rospy.sleep(sleep_dur)
#self._state_transitioning_lock.acquire()
# Determine outcome
outcome = self._loopback_name
for item in self._conditions:
(oc, cond) = item
if all(sn in self._returned_outcomes
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.on_exit(
self.userdata,
states=[
s for s in self._ordered_states
if s.name not in list(self._returned_outcomes.keys())
or self._returned_outcomes[s.name] == self._loopback_name
])
self._returned_outcomes = dict()
# right now, going out of a cc may break sync
# thus, as a quick fix, explicitly sync again
self._parent._inner_sync_request = True
self._current_state = None
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 _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))
