def _state_runner(self, label):
"""Runs the states in parallel threads."""
# Wait until all threads are ready to start before beginnging
self._ready_event.wait()
self.call_transition_cbs()
# Execute child state
try:
self._child_outcomes[label] = self._states[label].execute(
smach.Remapper(
self.userdata,
self._states[label].get_registered_input_keys(),
self._states[label].get_registered_output_keys(),
self._remappings[label]))
except PreemptException:
self._child_outcomes[label] = self._default_outcome
except:
self._user_code_exception = True
with self._done_cond:
self._done_cond.notify_all()
raise smach.InvalidStateError(
("Could not execute child state '%s': " % label) +
traceback.format_exc())
# Make sure the child returned an outcome
if self._child_outcomes[label] is None:
raise smach.InvalidStateError(
"Concurrent state '%s' returned no outcome on termination." %
label)
else:
smach.loginfo(
"Concurrent state '%s' returned outcome '%s' on termination." %
(label, self._child_outcomes[label]))
self._done_cond.acquire()
self.request_preempt()
self._done_cond.notify_all()
self._done_cond.release()
# Check if all of the states have completed
with self._done_cond:
# initialize preemption flag
preempt_others = False
# Call transition cb's
self.call_transition_cbs()
# Call child termination cb if it's defined
if self._child_termination_cb:
try:
preempt_others = self._child_termination_cb(
self._child_outcomes)
except:
raise smach.InvalidUserCodeError(
"Could not execute child termination callback: " +
traceback.format_exc())
# Notify the container to terminate (and preempt other states if neceesary)
if preempt_others or all(
[o is not None for o in self._child_outcomes.values()]):
self._done_cond.notify_all()
def _currently_opened_container(cls):
"""Get the currently opened container task.
This also asserts that the open container task is of type cls.
"""
if ContainerMixin._any_containers_opened():
opened_container = ContainerMixin._construction_stack[-1]
if not isinstance(opened_container, cls):
raise smach.InvalidStateError('Attempting to call a %s construction method without having opened a %s.' % (cls, cls))
return opened_container
else:
raise smach.InvalidStateError('Attempting to access the currently opened container task, but no container task is opened.')
def add_auto(label, state, connector_outcomes, transitions = None, remapping = None):
"""Add a state to the state machine such that it automatically transitions to the next added state.
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 = StateMachine._currently_opened_container()
# First add this state
add_ret = smach.StateMachine.add(label, state, transitions, remapping)
# Make sure the connector outcomes are valid for this state
registered_outcomes = state.get_registered_outcomes()
if not all([co in registered_outcomes for co in connector_outcomes]):
raise smach.InvalidStateError("Attempting to auto-connect states with outcomes %s, but state '%s' only has registerd outcomes: %s" % (str(connector_outcomes), str(label), str(registered_outcomes)))
# Store this state as the last state and store the connector outcomes
self._last_added_label = label
self._connector_outcomes = connector_outcomes
return add_ret
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 _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,
# Service info
service_name,
service_spec,
# Request Policy
request=None,
request_cb=None,
request_cb_args=[],
request_cb_kwargs={},
request_key=None,
request_slots=[],
# Response Policy
response_cb=None,
response_cb_args=[],
response_cb_kwargs={},
response_key=None,
response_slots=[],
# Keys
input_keys=[],
output_keys=[],
outcomes=[],
):
smach.State.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'])
# Store Service info
self._service_name = service_name
self._service_spec = service_spec
self._proxy = None
# Store request policy
if request is None:
self._request = service_spec._request_class()
else:
self._request = request
if request_cb is not None and not hasattr(request_cb, '__call__'):
raise smach.InvalidStateError(
"Request callback object given to ServiceState that IS NOT a function object"
)
self._request_cb = request_cb
self._request_cb_args = request_cb_args
self._request_cb_kwargs = request_cb_kwargs
if smach.has_smach_interface(request_cb):
self._request_cb_input_keys = request_cb.get_registered_input_keys(
)
self._request_cb_output_keys = request_cb.get_registered_output_keys(
)
self.register_input_keys(self._request_cb_input_keys)
self.register_output_keys(self._request_cb_output_keys)
else:
self._request_cb_input_keys = input_keys
self._request_cb_output_keys = output_keys
self._request_key = request_key
if request_key is not None:
self.register_input_keys([request_key])
self._request_slots = request_slots
self.register_input_keys(request_slots)
# Store response policy
if response_cb is not None and not hasattr(response_cb, '__call__'):
raise smach.InvalidStateError(
"Response callback object given to ServiceState that IS NOT a function object"
)
self._response_cb = response_cb
self._response_cb_args = response_cb_args
self._response_cb_kwargs = response_cb_kwargs
if smach.has_smach_interface(response_cb):
self._response_cb_input_keys = response_cb.get_registered_input_keys(
)
self._response_cb_output_keys = response_cb.get_registered_output_keys(
)
self._response_cb_outcomes = response_cb.get_registered_outcomes()
self.register_input_keys(self._response_cb_input_keys)
self.register_output_keys(self._response_cb_output_keys)
self.register_outcomes(self._response_cb_outcomes)
else:
self._response_cb_input_keys = input_keys
self._response_cb_output_keys = output_keys
self._response_cb_outcomes = outcomes
# Register additional input and output keys
self.register_input_keys(input_keys)
self.register_output_keys(output_keys)
self.register_outcomes(outcomes)
self._response_key = response_key
if response_key is not None:
self.register_output_keys([response_key])
self._response_slots = response_slots
self.register_output_keys(response_slots)
def __init__(
self,
# Action info
action_name,
action_spec,
# Default goal
goal=None,
goal_key=None,
goal_slots=[],
goal_cb=None,
goal_cb_args=[],
goal_cb_kwargs={},
# Result modes
result_key=None,
result_slots=[],
result_cb=None,
result_cb_args=[],
result_cb_kwargs={},
# Keys
input_keys=[],
output_keys=[],
outcomes=[],
# Timeouts
exec_timeout=None,
cancel_timeout=rospy.Duration(15.0),
server_wait_timeout=rospy.Duration(60.0),
):
"""Constructor for SimpleActionState action client wrapper.
@type action_name: string
@param action_name: The name of the action as it will be broadcast over ros.
@type action_spec: actionlib action msg
@param action_spec: The type of action to which this client will connect.
@type goal: actionlib goal msg
@param goal: If the goal for this action does not need to be generated at
runtime, it can be passed to this state on construction.
@type goal_key: string
@param goal_key: Pull the goal message from a key in the userdata.
This will be done before calling the goal_cb if goal_cb is defined.
@type goal_slots: list of string
@param goal_slots: Pull the goal fields (__slots__) from like-named
keys in userdata. This will be done before calling the goal_cb if
goal_cb is defined.
@type goal_cb: callable
@param goal_cb: If the goal for this action needs to be generated at
runtime, a callback can be stored which modifies the default goal
object. The callback is passed two parameters:
- userdata
- current stored goal
The callback returns a goal message.
@type result_key: string
@param result_key: Put the result message into the userdata with
the given key. This will be done after calling the result_cb
if result_cb is defined.
@type result_slots: list of strings
@param result_slots: Put the result message fields (__slots__)
into the userdata with keys like the field names. This will be done
after calling the result_cb if result_cb is defined.
@type result_cb: callable
@param result_cb: If result information from this action needs to be
stored or manipulated on reception of a result from this action, a
callback can be stored which is passed this information. The callback
is passed three parameters:
- userdata (L{UserData<smach.user_data.UserData>})
- result status (C{actionlib.GoalStatus})
- result (actionlib result msg)
@type exec_timeout: C{rospy.Duration}
@param exec_timeout: This is the timeout used for sending a preempt message
to the delegate action. This is C{None} by default, which implies no
timeout.
@type cancel_timeout: C{rospy.Duration}
@param cancel_timeout: This is the timeout used for aborting the state after a
preempt has been requested or the execution timeout occured and no result
from the action has been received. This timeout begins counting after cancel
to the action server has been sent.
@type server_wait_timeout: C{rospy.Duration}
@param server_wait_timeout: This is the timeout used for aborting while
waiting for an action server to become active.
"""
# Initialize base class
State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
# Set action properties
self._action_name = action_name
self._action_spec = action_spec
# Set timeouts
self._goal_status = 0
self._goal_result = None
self._exec_timeout = exec_timeout
self._cancel_timeout = cancel_timeout
self._server_wait_timeout = server_wait_timeout
# Set goal generation policy
if goal and hasattr(goal, '__call__'):
raise smach.InvalidStateError(
"Goal object given to SimpleActionState that IS a function object"
)
sl = action_spec().action_goal.goal.__slots__
if not all([s in sl for s in goal_slots]):
raise smach.InvalidStateError(
"Goal slots specified are not valid slots. Available slots: %s; specified slots: %s"
% (sl, goal_slots))
if goal_cb and not hasattr(goal_cb, '__call__'):
raise smach.InvalidStateError(
"Goal callback object given to SimpleActionState that IS NOT a function object"
)
# Static goal
if goal is None:
self._goal = copy.copy(action_spec().action_goal.goal)
else:
self._goal = goal
# Goal from userdata key
self._goal_key = goal_key
if goal_key is not None:
self.register_input_keys([goal_key])
# Goal slots from userdata keys
self._goal_slots = goal_slots
self.register_input_keys(goal_slots)
# Goal generation callback
self._goal_cb = goal_cb
self._goal_cb_args = goal_cb_args
self._goal_cb_kwargs = goal_cb_kwargs
if smach.has_smach_interface(goal_cb):
self._goal_cb_input_keys = goal_cb.get_registered_input_keys()
self._goal_cb_output_keys = goal_cb.get_registered_output_keys()
self.register_input_keys(self._goal_cb_input_keys)
self.register_output_keys(self._goal_cb_output_keys)
else:
self._goal_cb_input_keys = input_keys
self._goal_cb_output_keys = output_keys
# Set result processing policy
if result_cb and not hasattr(result_cb, '__call__'):
raise smach.InvalidStateError(
"Result callback object given to SimpleActionState that IS NOT a function object"
)
if not all([
s in action_spec().action_result.result.__slots__
for s in result_slots
]):
raise smach.InvalidStateError(
"Result slots specified are not valid slots.")
# Result callback
self._result_cb = result_cb
self._result_cb_args = result_cb_args
self._result_cb_kwargs = result_cb_kwargs
if smach.has_smach_interface(result_cb):
self._result_cb_input_keys = result_cb.get_registered_input_keys()
self._result_cb_output_keys = result_cb.get_registered_output_keys(
)
self._result_cb_outcomes = result_cb.get_registered_outcomes()
self.register_input_keys(self._result_cb_input_keys)
self.register_output_keys(self._result_cb_output_keys)
self.register_outcomes(self._result_cb_outcomes)
else:
self._result_cb_input_keys = input_keys
self._result_cb_output_keys = output_keys
self._result_cb_outcomes = outcomes
# Result to userdata key
self._result_key = result_key
if result_key is not None:
self.register_output_keys([result_key])
# Result slots to userdata keys
self._result_slots = result_slots
self.register_output_keys(result_slots)
# Register additional input and output keys
self.register_input_keys(input_keys)
self.register_output_keys(output_keys)
self.register_outcomes(outcomes)
# Declare some status variables
self._activate_time = rospy.Time.now()
self._cancel_time = rospy.Time.now()
self._duration = rospy.Duration(0.0)
self._status = SimpleActionState.WAITING_FOR_SERVER
# Construct action client, and wait for it to come active
self._action_client = SimpleActionClient(action_name, action_spec)
self._action_wait_thread = threading.Thread(
name=self._action_name + '/wait_for_server',
target=self._wait_for_server)
self._action_wait_thread.start()
self._execution_timer_thread = None
self._cancelation_timer_thread = None
# Condition variables for threading synchronization
self._done_cond = threading.Condition()
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,
outcomes,
default_outcome,
input_keys=[],
output_keys=[],
outcome_map={},
outcome_cb=None,
child_termination_cb=None):
"""Constructor for smach Concurrent Split.
@type outcomes: list of strings
@param outcomes: The potential outcomes of this state machine.
@type default_outcome: string
@param default_outcome: The outcome of this state if no elements in the
outcome map are satisfied by the outcomes of the contained states.
@type outcome_map: list
@param outcome_map: This is an outcome map for determining the
outcome of this container. Each outcome of the container is mapped
to a dictionary mapping child labels onto outcomes. If none of the
child-outcome maps is satisfied, the concurrence will terminate
with thhe default outcome.
For example, if the and_outcome_map is:
{'succeeded' : {'FOO':'succeeded', 'BAR':'done'},
'aborted' : {'FOO':'aborted'}}
Then the concurrence will terimate with outcome 'succeeded' only if
BOTH states 'FOO' and 'BAR' have terminated
with outcomes 'succeeded' and 'done', respectively. The outcome
'aborted' will be returned by the concurrence if the state 'FOO'
returns the outcome 'aborted'.
If the outcome of a state is not specified, it will be treated as
irrelevant to the outcome of the concurrence
If the criteria for one outcome is the subset of another outcome,
the container will choose the outcome which has more child outcome
criteria satisfied. If both container outcomes have the same
number of satisfied criteria, the behavior is undefined.
If a more complex outcome policy is required, see the user can
provide an outcome callback. See outcome_cb, below.
@type child_termination_cb: callale
@param child_termination_cb: This callback gives the user the ability
to force the concurrence to preempt running states given the
termination of some other set of states. This is useful when using
a concurrence as a monitor container.
This callback is called each time a child state terminates. It is
passed a single argument, a dictionary mapping child state labels
onto their outcomes. If a state has not yet terminated, it's dict
value will be None.
This function can return three things:
- False: continue blocking on the termination of all other states
- True: Preempt all other states
- list of state labels: Preempt only the specified states
I{If you just want the first termination to cause the other children
to terminate, the callback (lamda so: True) will always return True.}
@type outcome_cb: callable
@param outcome_cb: If the outcome policy needs to be more complicated
than just a conjunction of state outcomes, the user can supply
a callback for specifying the outcome of the container.
This callback is called only once all child states have terminated,
and it is passed the dictionary mapping state labels onto their
respective outcomes.
If the callback returns a string, it will treated as the outcome of
the container.
If the callback returns None, the concurrence will first check the
outcome_map, and if no outcome in the outcome_map is satisfied, it
will return the default outcome.
B{NOTE: This callback should be a function ONLY of the outcomes of
the child states. It should not access any other resources.}
"""
smach.container.Container.__init__(self, outcomes, input_keys,
output_keys)
# List of concurrent states
self._states = {}
self._threads = {}
self._remappings = {}
if not (default_outcome or outcome_map or outcome_cb):
raise smach.InvalidStateError(
"Concurrence requires an outcome policy")
# Initialize error string
errors = ""
# Check if default outcome is necessary
if default_outcome != str(default_outcome):
errors += "\n\tDefault outcome '%s' does not appear to be a string." % str(
default_outcome)
if default_outcome not in outcomes:
errors += "\n\tDefault outcome '%s' is unregistered." % str(
default_outcome)
# Check if outcome maps only contain outcomes that are registered
for o in outcome_map:
if o not in outcomes:
errors += "\n\tUnregistered outcome '%s' in and_outcome_map." % str(
o)
# Check if outcome cb is callable
if outcome_cb and not hasattr(outcome_cb, '__call__'):
errors += "\n\tOutcome callback '%s' is not callable." % str(
outcome_cb)
# Check if child termination cb is callable
if child_termination_cb and not hasattr(child_termination_cb,
'__call__'):
errors += "\n\tChild termination callback '%s' is not callable." % str(
child_termination_cb)
# Report errors
if len(errors) > 0:
raise smach.InvalidStateError(
"Errors specifying outcome policy of concurrence: %s" % errors)
# Store outcome policies
self._default_outcome = default_outcome
self._outcome_map = outcome_map
self._outcome_cb = outcome_cb
self._child_termination_cb = child_termination_cb
self._child_outcomes = {}
# Condition variables for threading synchronization
self._user_code_exception = False
self._done_cond = threading.Condition()
self._ready_event = threading.Event()
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 = {o: None for o in registered_outcomes if o not in transitions}
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
