def execute(self, userdata):
# Call callbacks
for (cb, ik, ok) in zip(self._cbs, self._cb_input_keys,
self._cb_output_keys):
# Call callback with limited userdata
try:
cb_outcome = cb(self, smach.Remapper(userdata, ik, ok, {}))
except:
cb_outcome = cb(smach.Remapper(userdata, ik, ok, {}))
return 'succeeded'
def makeRequest(self, userdata):
# Grab request key if set
if self._request_key is not None:
if not self._request_key in userdata:
rospy.logerr(
"Requested request key '%s' not in userdata structure. Available keys are: %s"
% (self._request_key, str(list(userdata.keys()))))
return False
self._request = userdata[self._request_key]
# Write request fields from userdata if set
for key in self._request_slots:
if key not in userdata:
rospy.logerr(
"Requested request slot key '%s' is not in userdata structure. Available keys are: %s"
% (key, str(list(userdata.keys()))))
return False
setattr(self._request, key, userdata[key])
# Call user-supplied callback, if set, to get a request
if self._request_cb is not None:
try:
self._request = self._request_cb(
smach.Remapper(userdata, self._request_cb_input_keys,
self._request_cb_output_keys, []),
self._request, *self._request_cb_args,
**self._request_cb_kwargs)
return True
except:
rospy.logerr("Could not execute request callback: " +
traceback.format_exc())
return False
return True
def handleResponse(self, response, userdata):
# Call response callback if it's set
response_cb_outcome = None
if self._response_cb is not None:
try:
response_cb_outcome = self._response_cb(
smach.Remapper(userdata, self._response_cb_input_keys,
self._response_cb_output_keys, []),
response, *self._response_cb_args,
**self._response_cb_kwargs)
if response_cb_outcome is not None and response_cb_outcome not in self.get_registered_outcomes(
):
rospy.logerr(
"Result callback for servicf {}, {} was not registered with the response_cb_outcomes argument. The response callback returned '{}' but the only registered outcomes are: {}"
.format(self._service_name, self._response_cb,
response_cb_outcome,
self.get_registered_outcomes()))
return 'aborted'
except:
rospy.logerr("Could not execute response callback: " +
traceback.format_exc())
return 'aborted'
if self._response_key is not None:
userdata[self._response_key] = response
for key in self._response_slots:
userdata[key] = getattr(response, key)
if response_cb_outcome is not None:
return response_cb_outcome
return 'succeeded'
def on_exit(self, userdata):
if self._current_state is not None:
ud = smach.Remapper(
self.userdata, self._current_state.get_registered_input_keys(),
self._current_state.get_registered_output_keys(),
self._remappings[self._current_state.name])
self._current_state.on_exit(ud)
self._current_state = None
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 _try_immediate_child(self, idx, userdata):
assert(userdata is not None)
task = self._tasks[idx]
return task.try_immediate(
smach.Remapper(
userdata,
task.get_registered_input_keys(),
task.get_registered_output_keys(),
self._remappings[idx]))
def execute(self, userdata):
for input_key in self._input_keys:
smach.loginfo('Userdata input key \'{}\' BEFORE callback execution: {}'.format(input_key, userdata[input_key]))
# Call callbacks
for (cb, ik, ok) in zip(self._cbs,
self._cb_input_keys,
self._cb_output_keys):
# Call callback with limited userdata
try:
cb_outcome = cb(self, smach.Remapper(userdata,ik,ok,{}))
except:
cb_outcome = cb(smach.Remapper(userdata,ik,ok,{}))
for input_key in self._input_keys:
smach.loginfo('Userdata input key \'{}\' AFTER callback execution: {}'.format(input_key, userdata[input_key]))
return 'succeeded'
def _execute_deferred_child(self, idx, parent_cb, userdata):
assert(userdata is not None)
task = self._tasks[idx]
task.execute_deferred(
parent_cb,
smach.Remapper(
userdata,
task.get_registered_input_keys(),
task.get_registered_output_keys(),
self._remappings[idx]))
def execute(self, userdata):
# Call callbacks
for (cb, ik, ok) in zip(self._cbs, self._cb_input_keys,
self._cb_output_keys):
# Call callback with limited userdata
try:
cb_outcome = cb(self, smach.Remapper(userdata, ik, ok, {}))
except:
cb_outcome = cb(smach.Remapper(userdata, ik, ok, {}))
# Get filename from userdata
try:
bag_file = userdata.file
assert (isinstance(bag_file, str))
except Exception as e:
rospy.logerr(
'The rosbag filename must be specified as a userdata input key: {}'
.format(repr(e)))
return 'aborted'
# Get topic names from userdata
try:
topics = userdata.topics
assert (not any(not isinstance(x, str) for x in topics))
except Exception as e:
rospy.logerr(
'Topic names must be specified as a userdata input key: {}'.
format(repr(e)))
return 'aborted'
# Start or stop recording
outcome = 'aborted'
if self._action == 'start' or self._action == 'record':
outcome = self._bag_recorder.start(bag_file, topics)
elif self._action == 'stop':
outcome = self._bag_recorder.stop(bag_file)
elif self._action == 'stop_all':
outcome = self._bag_recorder.stop_all()
return outcome
def execute(self, parent_ud=smach.UserData()):
outcome = super(DoOnExit, self).execute(parent_ud)
# run all finally states associated to the parent's outcome, and pass the outcome through
for label, state in self._exit_states[outcome] + self._exit_states[
'__ANY__']:
state.execute(
smach.Remapper(self.userdata,
state.get_registered_input_keys(),
state.get_registered_output_keys(),
self._remappings[label]))
return outcome
def on_exit(self, userdata):
for name, state in self._states.items():
if isinstance(state, smach.StateMachine): continue
state.on_exit(
smach.Remapper(
userdata, state.get_registered_input_keys(),
state.get_registered_output_keys(), {
key: name + '_' + key
for key in state.get_registered_input_keys() +
state.get_registered_output_keys()
}))
state._entering = True
def _goal_feedback_cb(self, feedback):
"""Goal Feedback Callback"""
rospy.logdebug("Action "+self._action_name+" sent feedback.")
if self._feedback_cb is not None:
self._feedback_cb(
smach.Remapper(
self._ud,
self._feedback_cb_input_keys,
self._feedback_cb_output_keys,
[]),
feedback,
*self._feedback_cb_args,
**self._feedback_cb_kwargs)
def execute(self, userdata):
# execute all active states
for name, state in self._states.items():
if self._returned_outcomes.has_key(name): continue
outcome = None
if isinstance(state, smach.StateMachine):
rmp_ud = smach.Remapper(
userdata, state.get_registered_input_keys(),
state.get_registered_output_keys(), {
key: name + '_' + key
for key in state.get_registered_input_keys() +
state.get_registered_output_keys()
})
state.userdata.merge(rmp_ud, rmp_ud.keys(), dict())
with state._state_transitioning_lock:
outcome = state._update_once()
else:
outcome = state.execute(
smach.Remapper(
userdata, state.get_registered_input_keys(),
state.get_registered_output_keys(), {
key: name + '_' + key
for key in state.get_registered_input_keys() +
state.get_registered_output_keys()
}))
if outcome is not None and outcome != "loopback":
Logger.loginfo("Concurrency: %s > %s" % (name, outcome))
self._returned_outcomes[name] = outcome
# determine outcome
for element in self._outcome_mapping:
state_outcome = element['outcome']
mapping = element['condition']
if all(name in self._returned_outcomes
and self._returned_outcomes[name] is outcome
for name, outcome in mapping.items()):
return state_outcome
def try_immediate(self, userdata):
# TODO provide a callback on message receipt instead of / as well as on
# activation?
# TODO allow activation callback to return Task.DEFERRED?
# Call recv callback if it's set
outcome = None
if self._exec_cb is not None:
try:
outcome = self._exec_cb(
smach.Remapper(userdata, self._exec_cb_input_keys,
self._exec_cb_output_keys, []),
self._last_msg, *self._exec_cb_args,
**self._exec_cb_kwargs)
if outcome not in [Task.ABORTED, Task.SUCCEEDED]:
rospy.logerr(
"Result callback for topic " + self._topic_name +
", " + str(self._exec_cb) +
" was not registered with the outcomes argument. The exec callback returned '"
+ str(outcome) +
"' but the only registered outcomes are Task.ABORTED andTask.SUCCEEDED"
)
return Task.ABORTED
except:
rospy.logerr("Could not execute exec callback: " +
traceback.format_exc())
return Task.ABORTED
if self._exec_key is not None:
userdata[self._exec_key] = self._last_msg
if self._last_msg is not None:
for key in self._exec_slots:
userdata[key] = getattr(self._last_msg, key)
if outcome is None:
outcome = Task.ABORTED if self._last_msg is None else Task.SUCCEEDED
# On next activation, if we haven't received any new messages, we return
# ABORTED. This behavior can be changed by the user callback (by
# stashing the previous message and storing it in userdata if the
# provided message is None)
self._last_msg = None
return outcome
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 execute_cb(self, goal):
"""Action server goal callback
This method is called when the action server associated with this state
machine receives a goal. This puts the goal into the userdata,
which is the userdata of the contained state.
"""
# If the state machine is running, we should preempt it before executing it
# it again.
rospy.logdebug("Starting wrapped SMACH container")
# Accept goal
#goal = self._action_server.accept_new_goal()
# Expand the goal into the root userdata for this server
if self._expand_goal_slots:
for slot in goal.__slots__:
self.userdata[slot] = getattr(goal, slot)
# Store the goal in the container local userdate
self.userdata[self._goal_key] = goal
# Store mapped goal slots in local userdata
for from_key, to_key in ((k, self._goal_slots_map[k])
for k in self._goal_slots_map):
self.userdata[to_key] = getattr(goal, from_key)
# Run the state machine (this blocks)
try:
container_outcome = self.wrapped_container.execute(
smach.Remapper(
self.userdata,
self.wrapped_container.get_registered_input_keys(),
self.wrapped_container.get_registered_output_keys(), {}))
except smach.InvalidUserCodeError as ex:
rospy.logerr("Exception thrown while executing wrapped container.")
self._action_server.set_aborted()
return
except:
rospy.logerr(
"Exception thrown:while executing wrapped container: " +
traceback.format_exc())
self._action_server.set_aborted()
return
# Grab the (potentially) populated result from the userdata
result = self.userdata[self._result_key]
# Store mapped slots in result
for from_key, to_key in ((k, self._result_slots_map)
for k in self._result_slots_map):
setattr(result, from_key, self.userdata[to_key])
# If any of the result members have been returned to the parent ud
# scope, overwrite the ones from the full structure
if self._pack_result_slots:
for slot in result.__slots__:
if slot in self.userdata:
setattr(result, slot, self.userdata[slot])
# Set terminal state based on state machine state outcome
if container_outcome in self._succeeded_outcomes:
rospy.loginfo('SUCCEEDED')
self._action_server.set_succeeded(result)
elif container_outcome in self._preempted_outcomes:
rospy.loginfo('PREEMPTED')
self._action_server.set_preempted(result)
else: #if container_outcome in self._aborted_outcomes:
rospy.loginfo('ABORTED')
self._action_server.set_aborted(result)
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, userdata):
# Call callbacks
for (cb, ik, ok) in zip(self._cbs, self._cb_input_keys,
self._cb_output_keys):
# Call callback with limited userdata
try:
cb_outcome = cb(self, smach.Remapper(userdata, ik, ok, {}))
except:
cb_outcome = cb(smach.Remapper(userdata, ik, ok, {}))
# Start or stop the message publisher
outcome = 'aborted'
if self._action == 'start':
# Parse msg
try:
if 'msg_type' in self._input_keys:
published_msg = self._parse_msg(userdata.msg,
msg_type=userdata.msg_type)
else:
published_msg = self._parse_msg(userdata.msg)
except Exception as e:
rospy.logerr('Failed to parse message: '.format(repr(e)))
return 'aborted'
# Get topic if it's specified as an input key
if 'topic' in self._input_keys:
topic = userdata.topic
# Otherwise, construct it from the state name
else:
topic = 'smacha/' + self._name.lower()
# Get rate if it's specified as an input key
if 'rate' in self._input_keys:
rate = userdata.rate
else:
rate = 100.0
# Get callback if it's specified as an input key
if 'callback' in self._input_keys:
callback = userdata.callback
else:
callback = ''
# Get frame_id if it's specified as an input key
if 'frame_id' in self._input_keys:
frame_id = userdata.frame_id
else:
frame_id = ''
# Start the publisher
outcome = self._msg_publisher.start(published_msg,
topic,
rate,
frame_id=frame_id,
callback=callback)
elif self._action == 'stop':
outcome = self._msg_publisher.stop(topic)
elif self._action == 'stop_all':
outcome = self._msg_publisher.stop_all()
# Set topic output key if specified
if self._action == 'start' and outcome == 'succeeded':
for output_key in ['topic', 'output_topic', 'topic_output']:
if output_key in self._output_keys:
setattr(userdata, output_key, topic)
# Set msg output key if specified
if self._action == 'start' and outcome == 'succeeded':
for output_key in ['msg', 'output_msg', 'msg_output']:
if output_key in self._output_keys:
setattr(userdata, output_key, published_msg)
return outcome
def execute(self, ud):
"""Called when executing a state.
This calls the goal_cb if it is defined, and then dispatches the
goal with a non-blocking call to the action client.
"""
# Make sure we're connected to the action server
if self._status is SimpleActionState.WAITING_FOR_SERVER:
rospy.logwarn(
"Still waiting for action server '%s' to start... is it running?"
% self._action_name)
if self._action_wait_thread.is_alive():
# Block on joining the server wait thread (This can be preempted)
self._action_wait_thread.join()
else:
# Wait for the server in this thread (This can also be preempted)
self._wait_for_server()
if not self.preempt_requested():
# In case of preemption we probably didn't connect
rospy.loginfo("Connected to action server '%s'." %
self._action_name)
# Check if server is still available
if self._status is SimpleActionState.INACTIVE:
try:
if not self._action_client.wait_for_server(
rospy.Duration(1.0)):
rospy.logerr("Failed to wait for action server '%s'" %
(self._action_name))
return 'aborted'
except:
if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called
rospy.logerr("Failed to wait for action server '%s'" %
(self._action_name))
return 'aborted'
# Check for preemption before executing
if self.preempt_requested():
rospy.loginfo("Preempting %s before sending goal." %
self._action_name)
self.service_preempt()
return 'preempted'
# We should only get here if we have connected to the server
if self._status is SimpleActionState.WAITING_FOR_SERVER:
rospy.logfatal("Action server for " + self._action_name +
" is not running.")
return 'aborted'
else:
self._status = SimpleActionState.INACTIVE
# Grab goal key, if set
if self._goal_key is not None:
self._goal = ud[self._goal_key]
# Write goal fields from userdata if set
for key in self._goal_slots:
setattr(self._goal, key, ud[key])
# Call user-supplied callback, if set, to get a goal
if self._goal_cb is not None:
try:
goal_update = self._goal_cb(
smach.Remapper(ud, self._goal_cb_input_keys,
self._goal_cb_output_keys, []), self._goal,
*self._goal_cb_args, **self._goal_cb_kwargs)
if goal_update is not None:
self._goal = goal_update
except:
rospy.logerr("Could not execute goal callback: " +
traceback.format_exc())
return 'aborted'
# Make sure the necessary paramters have been set
if self._goal is None and self._goal_cb is None:
rospy.logerr(
"Attempting to activate action " + self._action_name +
" with no goal or goal callback set. Did you construct the SimpleActionState properly?"
)
return 'aborted'
# Dispatch goal via non-blocking call to action client
self._activate_time = rospy.Time.now()
self._status = SimpleActionState.ACTIVE
# Wait on done condition
self._done_cond.acquire()
self._action_client.send_goal(self._goal, self._goal_done_cb,
self._goal_active_cb,
self._goal_feedback_cb)
# Preempt timeout watch thread
if self._exec_timeout:
self._execution_timer_thread = threading.Thread(
name=self._action_name + '/preempt_watchdog',
target=self._execution_timer)
self._execution_timer_thread.start()
# Wait for action to finish
self._done_cond.wait()
# Call user result callback if defined
result_cb_outcome = None
if self._result_cb is not None:
try:
result_cb_outcome = self._result_cb(
smach.Remapper(ud, self._result_cb_input_keys,
self._result_cb_output_keys, []),
self._goal_status, self._goal_result)
if result_cb_outcome is not None and result_cb_outcome not in self.get_registered_outcomes(
):
rospy.logerr(
"Result callback for action " + self._action_name +
", " + str(self._result_cb) +
" was not registered with the result_cb_outcomes argument. The result callback returned '"
+ str(result_cb_outcome) +
"' but the only registered outcomes are: " +
str(self.get_registered_outcomes()))
return 'aborted'
except:
rospy.logerr("Could not execute result callback: " +
traceback.format_exc())
return 'aborted'
if self._result_key is not None:
ud[self._result_key] = self._goal_result
for key in self._result_slots:
ud[key] = getattr(self._goal_result, key)
# Check status
if self._status == SimpleActionState.INACTIVE:
# Set the outcome on the result state
if self._goal_status == GoalStatus.SUCCEEDED:
outcome = 'succeeded'
elif self._goal_status == GoalStatus.PREEMPTED and self.preempt_requested(
):
outcome = 'preempted'
self.service_preempt()
else:
# All failures at this level are captured by aborting, even if we timed out
# This is an important distinction between local preemption, and preemption
# from above.
outcome = 'aborted'
else:
# We terminated without going inactive
rospy.logwarn(
"Action state terminated without going inactive first.")
outcome = 'aborted'
# Check custom result cb outcome
if result_cb_outcome is not None:
outcome = result_cb_outcome
# Set status inactive
self._status = SimpleActionState.INACTIVE
self._done_cond.release()
if self.preempt_requested():
outcome = 'preempted'
self.service_preempt()
return outcome
def execute(self, ud):
"""Called when executing a state.
This calls the goal_cb if it is defined, and then dispatches the
goal with a non-blocking call to the action client.
"""
self._status = SimpleActionState.WAITING_FOR_SERVER
# Wait for server
if not self._wait_for_server():
if self.preempt_requested():
rospy.loginfo("Preempting %s while waiting for server." % self._action_name)
self.service_preempt()
return 'preempted'
rospy.logfatal("Action server for "+self._action_name+" is not running.")
return 'unreachable'
# We did connect to server. Change status to INACTIVE
self._status = SimpleActionState.INACTIVE
# Check for preemption before executing
if self.preempt_requested():
rospy.loginfo("Preempting %s before sending goal." % self._action_name)
self.service_preempt()
return 'preempted'
# Grab goal key, if set
if self._goal_key is not None:
self._goal = ud[self._goal_key]
# Write goal fields from userdata if set
for key in self._goal_slots:
setattr(self._goal, key, ud[key])
# Call user-supplied callback, if set, to get a goal
if self._goal_cb is not None:
try:
goal_update = self._goal_cb(
smach.Remapper(
ud,
self._goal_cb_input_keys,
self._goal_cb_output_keys,
[]),
self._goal,
*self._goal_cb_args,
**self._goal_cb_kwargs)
if goal_update is not None:
self._goal = goal_update
except:
rospy.logerr("Could not execute goal callback: "+traceback.format_exc())
return 'aborted'
# Make sure the necessary paramters have been set
if self._goal is None and self._goal_cb is None:
rospy.logerr("Attempting to activate action "+self._action_name+" with no goal or goal callback set. Did you construct the SimpleActionState properly?")
return 'aborted'
# Dispatch goal via non-blocking call to action client
self._activate_time = rospy.Time.now()
self._status = SimpleActionState.ACTIVE
# Wait on done condition
self._done_cond.acquire()
self._action_client.send_goal(self._goal, self._goal_done_cb, self._goal_active_cb, self._goal_feedback_cb)
# Preempt timeout watch thread
if self._exec_timeout:
self._execution_timer_thread = threading.Thread(name=self._action_name+'/preempt_watchdog', target=self._execution_timer)
self._execution_timer_thread.start()
# Wait for action to finish
self._done_cond.wait()
# Call user result callback if defined
result_cb_outcome = None
if self._result_cb is not None:
try:
result_cb_outcome = self._result_cb(
smach.Remapper(
ud,
self._result_cb_input_keys,
self._result_cb_output_keys,
[]),
self._goal_status,
self._goal_result)
if result_cb_outcome is not None and result_cb_outcome not in self.get_registered_outcomes():
rospy.logerr("Result callback for action "+self._action_name+", "+str(self._result_cb)+" was not registered with the result_cb_outcomes argument. The result callback returned '"+str(result_cb_outcome)+"' but the only registered outcomes are: "+str(self.get_registered_outcomes()))
return 'aborted'
except:
rospy.logerr("Could not execute result callback: "+traceback.format_exc())
return 'aborted'
if self._result_key is not None:
ud[self._result_key] = self._goal_result
for key in self._result_slots:
ud[key] = getattr(self._goal_result, key)
# Check status
if self._status == SimpleActionState.INACTIVE:
# If the state was requested preempted we have to obey it by calling
# self.service_preempt() regardless of the actual goal status to prevent
# this State from ending up in an unfornate situation where it would preempt
# immediately on the next run (See line 278).
if self.preempt_requested():
outcome = 'preempted'
self.service_preempt()
# Set the outcome on the result state
if self._goal_status == GoalStatus.SUCCEEDED:
outcome = 'succeeded'
else:
# All failures at this level are captured by aborting, even if we timed out
# This is an important distinction between local preemption, and preemption
# from above.
outcome = 'aborted'
else:
# We terminated without going inactive
rospy.logwarn("Action state terminated without going inactive first.")
outcome = 'aborted'
# Check custom result cb outcome
if result_cb_outcome is not None:
outcome = result_cb_outcome
# Set status inactive
self._status = SimpleActionState.INACTIVE
self._done_cond.release()
return outcome
def execute(self, ud):
"""Execute service"""
# Check for preemption before executing
if self.preempt_requested():
rospy.loginfo("Preempting %s before sending request." %
self._service_name)
self.service_preempt()
return 'preempted'
# Make sure we're connected to the service
try:
while self._proxy is None:
if self.preempt_requested():
rospy.loginfo(
"Preempting while waiting for service '%s'." %
self._service_name)
self.service_preempt()
return 'preempted'
if rospy.is_shutdown():
rospy.loginfo(
"Shutting down while waiting for service '%s'." %
self._service_name)
return 'aborted'
try:
rospy.wait_for_service(self._service_name, 1.0)
self._proxy = rospy.ServiceProxy(self._service_name,
self._service_spec)
rospy.logdebug("Connected to service '%s'" %
self._service_name)
except rospy.ROSException as ex:
rospy.logwarn("Still waiting for service '%s'..." %
self._service_name)
except:
rospy.logwarn("Terminated while waiting for service '%s'." %
self._service_name)
return 'aborted'
# Grab request key if set
if self._request_key is not None:
if self._request_key in ud:
self._request = ud[self._request_key]
else:
rospy.logerr(
"Requested request key '%s' not in userdata struture. Available keys are: %s"
% (self._request_key, str(list(ud.keys()))))
return 'aborted'
# Write request fields from userdata if set
for key in self._request_slots:
if key in ud:
setattr(self._request, key, ud[key])
else:
rospy.logerr(
"Requested request slot key '%s' is not in userdata strcture. Available keys are: %s"
% (key, str(list(ud.keys()))))
return 'aborted'
# Call user-supplied callback, if set, to get a request
if self._request_cb is not None:
try:
request_update = self._request_cb(
smach.Remapper(ud, self._request_cb_input_keys,
self._request_cb_output_keys, []),
self._request, *self._request_cb_args,
**self._request_cb_kwargs)
if request_update is not None:
self._request = request_update
except:
rospy.logerr("Could not execute request callback: " +
traceback.format_exc())
return 'aborted'
if self._request is None:
rospy.logerr("Attempting to call service " + self._service_name +
" with no request")
return 'aborted'
# Call service
# Abandon hope, all ye who enter here
service_call_failed = False
try:
rospy.logdebug("Calling service %s with request:\n%s" %
(self._service_name, str(self._request)))
self._response = self._proxy(self._request)
except rospy.ServiceException as ex:
rospy.logerr("Exception when calling service '%s': %s" %
(self._service_name, str(ex)))
service_call_failed = True
# If the state was requested preempted while service call was in progress we
# need to service it to prevent this State from ending up in an unfornate situation
# where it would preempt immediately on the next run (See line 111).
if self.preempt_requested():
rospy.loginfo("Preempting %s after receiving a response." %
self._service_name)
self.service_preempt()
return 'preempted'
if service_call_failed:
return 'aborted'
# Call response callback if it's set
response_cb_outcome = None
if self._response_cb is not None:
try:
response_cb_outcome = self._response_cb(
smach.Remapper(ud, self._response_cb_input_keys,
self._response_cb_output_keys, []),
self._response, *self._response_cb_args,
**self._response_cb_kwargs)
if response_cb_outcome is not None and response_cb_outcome not in self.get_registered_outcomes(
):
rospy.logerr(
"Result callback for service " + self._service_name +
", " + str(self._response_cb) +
" was not registered with the response_cb_outcomes argument. The response callback returned '"
+ str(response_cb_outcome) +
"' but the only registered outcomes are: " +
str(self.get_registered_outcomes()))
return 'aborted'
except:
rospy.logerr("Could not execute response callback: " +
traceback.format_exc())
return 'aborted'
if self._response_key is not None:
ud[self._response_key] = self._response
for key in self._response_slots:
ud[key] = getattr(self._response, key)
if response_cb_outcome is not None:
return response_cb_outcome
return 'succeeded'
def execute(self, ud):
"""Execute service"""
# Check for preemption before executing
if self.preempt_requested():
self.node.get_logger().info(
"Preempting %s before sending request." % self._service_name)
self.service_preempt()
return 'preempted'
# Make sure we're connected to the service
try:
while not self._proxy.service_is_ready():
if self.preempt_requested():
self.node.get_logger().info(
"Preempting while waiting for service '%s'." %
self._service_name)
self.service_preempt()
return 'preempted'
if not rclpy.ok():
self.node.get_logger().info(
"Shutting down while waiting for service '%s'." %
self._service_name)
return 'aborted'
if self._proxy.wait_for_service(1.0):
self.node.get_logger().debug("Connected to service '%s'" %
self._service_name)
else:
self.node.get_logger().warn(
"Still waiting for service '%s'..." %
self._service_name)
except:
self.node.get_logger().warn(
"Terminated while waiting for service '%s'." %
self._service_name)
return 'aborted'
# Grab request key if set
if self._request_key is not None:
if self._request_key in ud:
self._request = ud[self._request_key]
else:
self.node.get_logger().error(
"Requested request key '%s' not in userdata struture. Available keys are: %s"
% (self._request_key, str(list(ud.keys()))))
return 'aborted'
# Write request fields from userdata if set
for key in self._request_slots:
if key in ud:
setattr(self._request, key, ud[key])
else:
self.node.get_logger().error(
"Requested request slot key '%s' is not in userdata strcture. Available keys are: %s"
% (key, str(list(ud.keys()))))
return 'aborted'
# Call user-supplied callback, if set, to get a request
if self._request_cb is not None:
try:
request_update = self._request_cb(
smach.Remapper(ud, self._request_cb_input_keys,
self._request_cb_output_keys, []),
self._request, *self._request_cb_args,
**self._request_cb_kwargs)
if request_update is not None:
self._request = request_update
except:
self.node.get_logger().error(
"Could not execute request callback: " +
traceback.format_exc())
return 'aborted'
if self._request is None:
self.node.get_logger().error("Attempting to call service " +
self._service_name +
" with no request")
return 'aborted'
# Call service
# Abandon hope, all ye who enter here
try:
self.node.get_logger().debug(
"Calling service %s with request:\n%s" %
(self._service_name, str(self._request)))
self._response = self._proxy.call(self._request)
except TypeError as ex:
self.node.get_logger().error(
"Exception when calling service '%s': %s" %
(self._service_name, str(ex)))
return 'aborted'
# Call response callback if it's set
response_cb_outcome = None
if self._response_cb is not None:
try:
response_cb_outcome = self._response_cb(
smach.Remapper(ud, self._response_cb_input_keys,
self._response_cb_output_keys, []),
self._response, *self._response_cb_args,
**self._response_cb_kwargs)
if response_cb_outcome is not None and response_cb_outcome not in self.get_registered_outcomes(
):
self.node.get_logger().error(
"Result callback for service " + self._service_name +
", " + str(self._response_cb) +
" was not registered with the response_cb_outcomes argument. The response callback returned '"
+ str(response_cb_outcome) +
"' but the only registered outcomes are: " +
str(self.get_registered_outcomes()))
return 'aborted'
except:
self.node.get_logger().error(
"Could not execute response callback: " +
traceback.format_exc())
return 'aborted'
if self._response_key is not None:
ud[self._response_key] = self._response
for key in self._response_slots:
ud[key] = getattr(self._response, key)
if response_cb_outcome is not None:
return response_cb_outcome
return 'succeeded'
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 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.
"""
#smach.loginfo(learner.q_table)
smach.loginfo(learner.q_table_additive_states)
# Initialize preempt state
self._preempted_label = None
self._preempted_state = None
current_label = self.state_label
# Set initial state. TODO:Do this using RL.
#val = random.randrange(0,len(self._states),1)
label_transition = learner.get_action(self.state_label)
smach.loginfo('CONTAINER LABEL: ' + str(self.state_label) +
' ACTION LABEL: ' + label_transition)
#smach.loginfo(val)
#label = self._labels[val]
self._set_current_state(label_transition)
#self._current_state.state_label = label_transition
next_label = label_transition
smach.loginfo('CONTAINER LABEL AFTER TRANSITION: ' +
str(label_transition))
self._states[label_transition].state_label = label_transition
#If the next-state is an RL-State, there is no reward until that state is executed. Add the current state to be updated
#by the next-states reward.
if next_label in learner.q_table.keys():
learner.queue_state_update(current_label, label_transition)
#smach.loginfo(self._states['START_NAV'].state_label)
# Copy input keys
self._copy_input_keys(parent_ud, self.userdata)
execution_outcome = self._current_state.execute((smach.Remapper(
self.userdata,
self._states[label_transition].get_registered_input_keys(),
self._states[label_transition].get_registered_output_keys(),
self._remappings[label_transition])))
if 'reward' in self.userdata:
reward = self.userdata['reward']
self.publisher.update_rl(current_label, -reward, label_transition,
self.container.num)
#learner.update(next_label, label_transition, current_label, -reward)
#learner.update_queued_states(next_label, -reward)
# Spew some info
#smach.loginfo("RL Stuff '%s' with userdata: \n\t%s" %
# (self._current_label, list(self.userdata.keys())))
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
# We're no longer running
self._is_running = False
found = False
for (container_outcome, outcomes) in ((k, self._outcome_map[k])
for k in self._outcome_map):
if execution_outcome in outcomes:
found = True
break
if found == False:
smach.loginfo("RL BAD OUTCOME %s", execution_outcome)
smach.loginfo('CONTAINER OUTCOME: ' + container_outcome +
' EXECUTION OUTCOME: ' + execution_outcome)
return container_outcome