def __init__(self,
input_keys=['outcome'],
output_keys=['outcome'],
callbacks={},
outcomes=['succeeded']):
smach.State.__init__(self,
input_keys=input_keys,
output_keys=output_keys,
outcomes=outcomes)
self._cbs = []
if callbacks:
for cb in sorted(callbacks):
if cb in globals():
self._cbs.append(globals()[cb])
elif cb in locals():
self._cbs.append(locals()[cb])
elif cb in dir(self):
self._cbs.append(getattr(self, cb))
self._cb_input_keys = []
self._cb_output_keys = []
self._cb_outcomes = []
for cb in self._cbs:
if cb and smach.has_smach_interface(cb):
self._cb_input_keys.append(cb.get_registered_input_keys())
self._cb_output_keys.append(cb.get_registered_output_keys())
self._cb_outcomes.append(cb.get_registered_outcomes())
self.register_input_keys(self._cb_input_keys[-1])
self.register_output_keys(self._cb_output_keys[-1])
self.register_outcomes(self._cb_outcomes[-1])
def __init__(
self,
cb,
topic_name,
msg_type,
global_timeout=None,
outcomes=["failed", "preempted"],
last_msg_timeout=None,
input_keys=[],
output_keys=[],
io_keys=[],
provide_self_instance=False,
sleep_duration=0.05,
):
if "failed" not in outcomes:
outcomes.append("failed")
smach.State.__init__(self, outcomes=outcomes, input_keys=input_keys, output_keys=output_keys, io_keys=io_keys)
# max time since beginning
self._global_timeout = global_timeout
# max time since last message : will not be needed when checking if the
# person grasps the hand of the robot
self._last_msg_timeout = last_msg_timeout
self._received_msg = None
self._beginning = None
self._last_msg_stamp = None
self._monitor_cb = cb
self._provide_self_instance = provide_self_instance
self._sleep_duration = sleep_duration
rospy.Subscriber(topic_name, msg_type, self.internal_callback)
# this is to be able to pass userdata to the internal_callback
if cb and smach.has_smach_interface(cb):
self.register_input_keys(cb.get_registered_input_keys())
self.register_output_keys(cb.get_registered_output_keys())
self.register_outcomes(cb.get_registered_outcomes())
def __init__(self, name, msg_publisher, action, input_keys = ['msg', 'topic', 'rate'], output_keys = ['msg', 'topic'], callbacks = None):
smach.State.__init__(self, input_keys=input_keys, output_keys=output_keys, outcomes=['succeeded', 'aborted'])
# Save the state name
self._name = name
# Save the MsgPublisherObserver object reference
self._msg_publisher = msg_publisher
# Save the action
self._action = action
# Set up dict of parsing functions for certain message types/classes.
self._msg_parsers = {"<class 'geometry_msgs.msg._Point.Point'>": parse_pointstamped,
"<class 'geometry_msgs.msg._PointStamped.PointStamped'>": parse_pointstamped,
"<class 'geometry_msgs.msg._Pose.Pose'>": parse_posestamped,
"<class 'geometry_msgs.msg._PoseStamped.PoseStamped'>": parse_posestamped,
"<class 'geometry_msgs.msg._PoseArray.PoseArray'>": parse_posearray,
"<class 'sensor_msgs.msg._PointCloud.PointCloud'>": parse_pointcloud,
"<class 'sensor_msgs.msg._PointCloud2.PointCloud2'>": parse_pointcloud2}
self._cbs = []
if callbacks:
for cb in sorted(callbacks):
if cb in globals():
self._cbs.append(globals()[cb])
elif cb in locals():
self._cbs.append(locals()[cb])
elif cb in dir(self):
self._cbs.append(getattr(self, cb))
self._cb_input_keys = []
self._cb_output_keys = []
self._cb_outcomes = []
for cb in self._cbs:
if cb and smach.has_smach_interface(cb):
self._cb_input_keys.append(cb.get_registered_input_keys())
self._cb_output_keys.append(cb.get_registered_output_keys())
self._cb_outcomes.append(cb.get_registered_outcomes())
self.register_input_keys(self._cb_input_keys[-1])
self.register_output_keys(self._cb_output_keys[-1])
self.register_outcomes(self._cb_outcomes[-1])
def __init__(self,
name,
bag_recorder,
action,
input_keys=['file', 'topics'],
output_keys=[],
callbacks=None):
smach.State.__init__(self,
input_keys=input_keys,
output_keys=output_keys,
outcomes=['succeeded', 'aborted'])
# Save the state name
self._name = name
# Save the ROSBagRecorder object reference
self._bag_recorder = bag_recorder
# Save the action
self._action = action
self._cbs = []
if callbacks:
for cb in sorted(callbacks):
if cb in globals():
self._cbs.append(globals()[cb])
elif cb in locals():
self._cbs.append(locals()[cb])
elif cb in dir(self):
self._cbs.append(getattr(self, cb))
self._cb_input_keys = []
self._cb_output_keys = []
self._cb_outcomes = []
for cb in self._cbs:
if cb and smach.has_smach_interface(cb):
self._cb_input_keys.append(cb.get_registered_input_keys())
self._cb_output_keys.append(cb.get_registered_output_keys())
self._cb_outcomes.append(cb.get_registered_outcomes())
self.register_input_keys(self._cb_input_keys[-1])
self.register_output_keys(self._cb_output_keys[-1])
self.register_outcomes(self._cb_outcomes[-1])
def __init__(
self,
cb,
topic_name,
msg_type,
outcomes=["done", "failed", "preempted"],
input_keys=[],
output_keys=[],
io_keys=[],
):
smach.State.__init__(self, outcomes=outcomes, input_keys=input_keys, output_keys=output_keys, io_keys=io_keys)
self._publisher = rospy.Publisher(topic_name, msg_type)
self._cb = cb
if cb and smach.has_smach_interface(cb):
self.register_input_keys(cb.get_registered_input_keys())
self.register_output_keys(cb.get_registered_output_keys())
self.register_outcomes(cb.get_registered_outcomes())
def __init__(self, actionname, actionspec,
goal = None,
goal_cb = None,
feedback_cb = None,
timeout= None):
smach.State.__init__(self,
input_keys=['button_cancel', 'sample'],
outcomes=['complete', 'timeout', 'canceled',
'preempted', 'aborted'])
self.actionname = actionname
self.actionspec = actionspec
self.goal = goal
self.goal_cb = goal_cb
self.feedback_cb = feedback_cb
self.timeout = timeout
#get keys from goal callback
if smach.has_smach_interface(goal_cb):
self.register_input_keys(goal_cb.get_registered_input_keys())
self.register_output_keys(goal_cb.get_registered_output_keys())
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 __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,
preempt_timeout = rospy.Duration(60.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 tobe 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 preempt_timeout: C{rospy.Duration}
@param preempt_timeout: This is the timeout used for aborting after a
preempt has been sent to the action and no result has been received. This
timeout begins counting after a preempt message 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._exec_timeout = exec_timeout
self._preempt_timeout = preempt_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")
if not all([s in action_spec().action_goal.goal.__slots__ for s in goal_slots]):
raise smach.InvalidStateError("Goal slots specified are not valid 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._preempt_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
# Condition variables for threading synchronization
self._done_cond = threading.Condition()
