def get_const(obj):
"""Get a const reference to an object if it has "user-defined" attributes."""
if hasattr(obj,'__dict__'):
smach.logdebug("Making const '%s'" % str(obj))
return Const(obj)
else:
return obj
def get_const(obj):
"""Get a const reference to an object if it has "user-defined" attributes."""
if hasattr(obj,'__dict__'):
smach.logdebug("Making const '%s'" % str(obj))
return Const(obj)
else:
return obj
def set_initial_state(self, initial_states, userdata=smach.UserData()):
smach.logdebug("Setting initial state to "+str(initial_states))
if len(initial_states) > 1:
smach.logwarn("Attempting to set initial state to include more than one state, but the StateMachine container can only have one initial state.")
# Set the initial state label
if len(initial_states) > 0:
self._initial_state_label = initial_states[0]
# Set local userdata
self.userdata.update(userdata)
def set_initial_state(self, initial_states, userdata=smach.UserData()):
smach.logdebug("Setting initial state to "+str(initial_states))
if len(initial_states) > 1:
smach.logwarn("Attempting to set initial state to include more than one state, but the StateMachine container can only have one initial state.")
# Set the initial state label
if len(initial_states) > 0:
self._initial_state_label = initial_states[0]
# Set local userdata
self.userdata.update(userdata)
def set_initial_state(self, initial_states, userdata=None):
"""Set initial active states of a container.
@type initial_states: list of string
@param initial_states: A description of the initial active state of this
container.
@type userdata: L{UserData}
@param userdata: Initial userdata for this container.
"""
with self._lock:
if userdata is None:
userdata = smach.UserData()
smach.logdebug("Setting initial states to " + str(initial_states))
if len(initial_states) > 1:
smach.logwarn(
"Attempting to set initial state to include more than"
" one state, but the BehaviorTreeContainer container can only"
" have one initial state. Taking the first one."
)
# Set the initial state label
if len(initial_states) > 0:
initial_state_label = initial_states[0]
# TODO We only support the root as initial state for now
# for proper support we'll want to walk the tree and set initial idx
# for every container on the path to the task with the specified
# label.
self._rebuild_unique_task_names()
try:
self._initial_task = self._task_for_label[initial_state_label]
except KeyError:
smach.logwarn("Unknown initial state '%s' requested", initial_state_label)
# Set local userdata
self.userdata.update(userdata)
def set_initial_state(self, initial_states, userdata=None):
"""Set initial active states of a container.
@type initial_states: list of string
@param initial_states: A description of the initial active state of this
container.
@type userdata: L{UserData}
@param userdata: Initial userdata for this container.
"""
with self._lock:
if userdata is None:
userdata = smach.UserData()
smach.logdebug("Setting initial states to " + str(initial_states))
if len(initial_states) > 1:
smach.logwarn("Attempting to set initial state to include more than"
" one state, but the BehaviorTreeContainer container can only"
" have one initial state. Taking the first one.")
# Set the initial state label
if len(initial_states) > 0:
initial_state_label = initial_states[0]
# TODO We only support the root as initial state for now
# for proper support we'll want to walk the tree and set initial idx
# for every container on the path to the task with the specified
# label.
self._rebuild_unique_task_names()
try:
self._initial_task = self._task_for_label[initial_state_label]
except KeyError:
smach.logwarn("Unknown initial state '%s' requested", initial_state_label)
# Set local userdata
self.userdata.update(userdata)
def add(task, block_on_preempt=True, remapping=None):
"""Add task to the opened container task.
block_on_preempt:
- in a parallel container, if we have an immediate result from one
of our tasks but we started another and have preempted it, then a
value of True for this parameter will mean we will block until the
preemption completes, and a value of False means we will return
RUNNING and return the result through the parent callback when the
preemption has completed.
- in a preempting sequential container, TODO
"""
smach.logdebug('Adding task (%s, %s)' % (task.get_label(), str(task)))
# Get currently opened container
self = ContainerTask._currently_opened_container()
# Store task
self._tasks.append(task)
self._remappings.append(remapping)
self._block_on_preempt.append(block_on_preempt)
return task
def __getitem__(self, name):
smach.logdebug("Getting '%s' from const wrapper." % name)
attr = self._obj[name]
return get_const(attr)
def __getattr__(self, name):
smach.logdebug("Getting '%s' from const wrapper." % name)
attr = getattr(self._obj,name)
return get_const(attr)
def 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)))
#If the state being added is an RL state.
if type(state) == smach.StateMachineRL:
state.add_learner(label, self)
# 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
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, obj):
smach.logdebug("Making const '%s'" % str(obj))
self._obj = obj
self.__initialized = True
def __getitem__(self, name):
smach.logdebug("Getting '%s' from const wrapper." % name)
attr = self._obj[name]
return get_const(attr)
def __getattr__(self, name):
smach.logdebug("Getting '%s' from const wrapper." % name)
attr = getattr(self._obj,name)
return get_const(attr)
def __init__(self, obj):
smach.logdebug("Making const '%s'" % str(obj))
self._obj = obj
self.__initialized = True
def add(label, state, transitions=None, remapping=None):
"""Add a state to the opened state machine.
@type label: string
@param label: The label of the state being added.
@param state: An instance of a class implementing the L{State} interface.
@param transitions: A dictionary mapping state outcomes to other state
labels or container outcomes.
@param remapping: A dictrionary mapping local userdata keys to userdata
keys in the container.
"""
# Get currently opened container
self = StateMachine._currently_opened_container()
smach.logdebug('Adding state (%s, %s, %s)' % (label, str(state), str(transitions)))
# Set initial state if it is still unset
if self._initial_state_label is None:
self._initial_state_label = label
if transitions is None:
transitions = {}
if remapping is None:
remapping = {}
# Add group transitions to this new state, if they exist
"""
if 'transitions' in smach.Container._context_kwargs:
for outcome, target in smach.Container._context_kwargs['transitions'].iteritems():
if outcome not in transitions:
transitions[outcome] = target
"""
# Check the state specification
self.check_state_spec(label, state, transitions)
# Check if th label already exists
if label in self._states:
raise smach.InvalidStateError(
'Attempting to add state with label "'+label+'" to state machine, but this label is already being used.')
# Debug info
smach.logdebug("Adding state '"+str(label)+"' to the state machine.")
# Create implicit terminal transitions, and combine them with the explicit transitions
registered_outcomes = state.get_registered_outcomes()
# Get a list of the unbound transitions
missing_transitions = {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