def __init__(self):
super(FoldArm,
self).__init__(outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='succeeded',
outcome_map={
'succeeded': {
'CLOSE_GRIPPER': 'succeeded',
'GOTO_RESTING': 'succeeded'
},
'preempted': {
'CLOSE_GRIPPER': 'preempted',
'GOTO_RESTING': 'preempted'
},
'aborted': {
'CLOSE_GRIPPER': 'aborted',
'GOTO_RESTING': 'aborted'
}
})
with self:
smach.Concurrence.add(
'CLOSE_GRIPPER',
smach_ros.SimpleActionState(
'gripper_controller/gripper_action',
control_msgs.GripperCommandAction,
goal=control_msgs.GripperCommandGoal(
control_msgs.GripperCommand(0.025, 0.0))))
smach.Concurrence.add('GOTO_RESTING', StoredConfig('resting'))
def FoldArm():
""" Concurrently fold arm and close the gripper """
sm = smach.Concurrence(outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='succeeded',
outcome_map={
'succeeded': {
'CloseGripper': 'succeeded',
'FoldArm': 'succeeded'
},
'preempted': {
'CloseGripper': 'preempted',
'FoldArm': 'preempted'
},
'aborted': {
'CloseGripper': 'aborted',
'FoldArm': 'aborted'
}
})
with sm:
smach.Concurrence.add(
'CloseGripper',
smach_ros.SimpleActionState('gripper_controller/gripper_action',
control_msgs.GripperCommandAction,
goal=control_msgs.GripperCommandGoal(
control_msgs.GripperCommand(
0.005, 0.0))))
smach.Concurrence.add(
'FoldArm',
smach_ros.SimpleActionState(
'move_to_target',
thorp_msgs.MoveToTargetAction,
goal=thorp_msgs.MoveToTargetGoal(
thorp_msgs.MoveToTargetGoal.NAMED_TARGET, 'resting', None,
None)))
return sm
def set_angle_target(self, position, max_effort=default_max_effort):
self.controller_pub.publish(
pcm.GripperCommand(position=position, max_effort=max_effort))
def set_angle(self, a, max_effort=default_max_effort):
self.grip_client.send_goal(
pcm.GripperCommandGoal(
pcm.GripperCommand(position=a, max_effort=max_effort)))
self.pr2.start_thread(Thread(target=self.grip_client.wait_for_result))