Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 PKG = 'sr_grasp'
00022 NAME = 'test_grasp_object'
00023
00024 import os, unittest;
00025 import rospy, rostest
00026 from actionlib import SimpleActionClient, GoalStatus
00027 from sr_robot_msgs.msg import GraspAction, GraspGoal
00028 from moveit_msgs.msg import Grasp
00029 from trajectory_msgs.msg import JointTrajectoryPoint
00030
00031 class TestGrasp(unittest.TestCase):
00032 longMessage = True
00033
00034 joint_names = [
00035 'LFJ0', 'LFJ3', 'LFJ4', 'LFJ5',
00036 'RFJ0', 'RFJ3', 'RFJ4',
00037 'MFJ0', 'MFJ3', 'MFJ4',
00038 'FFJ0', 'FFJ3', 'FFJ4',
00039 'THJ1', 'THJ2', 'THJ3', 'THJ4', 'THJ5',
00040 'WRJ1', 'WRJ2']
00041
00042 def mk_grasp(self, joints):
00043 grasp = Grasp()
00044
00045 grasp.pre_grasp_posture.joint_names = self.joint_names
00046 jtp = JointTrajectoryPoint()
00047 for jname in self.joint_names:
00048 jtp.positions.append(0.0)
00049 grasp.pre_grasp_posture.points.append(jtp)
00050
00051 grasp.grasp_posture.joint_names = self.joint_names
00052 jtp = JointTrajectoryPoint()
00053 for jname in self.joint_names:
00054 if jname in joints:
00055 jtp.positions.append(joints[jname])
00056 else:
00057 jtp.positions.append(0.0)
00058 grasp.grasp_posture.points.append(jtp)
00059 return grasp
00060
00061
00062
00063
00064
00065 def test_grasp_action(self):
00066 """Test sending a grasp."""
00067 goal = GraspGoal()
00068 goal.grasp = self.mk_grasp({
00069 'LFJ3': 1.4, 'RFJ3': 1.4, 'MFJ3': 1.4, 'FFJ3': 1.4,
00070 'LFJ0': 3.0, 'RFJ0': 3.0, 'MFJ0': 3.0, 'FFJ0': 3.0,
00071 })
00072
00073
00074 client = SimpleActionClient('grasp', GraspAction)
00075 client.wait_for_server()
00076
00077
00078 goal.pre_grasp = True
00079 client.send_goal(goal)
00080 client.wait_for_result(rospy.Duration.from_sec(20.0))
00081 self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED,
00082 "Action did not return in SUCCEEDED state.")
00083
00084 rospy.sleep(2)
00085
00086
00087 goal.pre_grasp = False
00088 client.send_goal(goal)
00089 client.wait_for_result(rospy.Duration.from_sec(20.0))
00090 self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED,
00091 "Action did not return in SUCCEEDED state.")
00092
00093
00094 if __name__ == '__main__':
00095 rospy.init_node(NAME)
00096 rostest.rosrun(PKG, NAME, TestGrasp)
00097