00001
00002
00003
00004 PKG = 'hironx_ros_bridge'
00005
00006 try:
00007 import roslib
00008 import hironx_ros_bridge
00009 except:
00010 import roslib; roslib.load_manifest(PKG)
00011 import hironx_ros_bridge
00012
00013 import rospy, actionlib, math, numpy
00014 import tf
00015 from tf.transformations import quaternion_matrix, euler_from_matrix
00016
00017 import unittest
00018
00019
00020 roslib.load_manifest('pr2_controllers_msgs')
00021 import pr2_controllers_msgs.msg
00022 import trajectory_msgs.msg
00023
00024 from sensor_msgs.msg import JointState
00025 from pr2_controllers_msgs.msg import JointTrajectoryAction
00026 from trajectory_msgs.msg import JointTrajectoryPoint
00027 from hrpsys_ros_bridge.srv import *
00028
00029 import time
00030 import tempfile
00031
00032 class TestHiroROSBridge(unittest.TestCase):
00033
00034 def joint_states_cb(self, msg):
00035 self.joint_states.append(msg)
00036 self.joint_states = self.joint_states[0:3000]
00037
00038 def __init__(self, *args, **kwargs):
00039 super(TestHiroROSBridge, self).__init__(*args, **kwargs)
00040
00041 self.joint_states = []
00042 rospy.init_node('hironx_ros_bridge_test')
00043 self.joint_states_sub = rospy.Subscriber("joint_states", JointState, self.joint_states_cb)
00044 self.filename_base = tempfile.mkstemp()[1]
00045 self.filenames = []
00046
00047 @classmethod
00048 def setUpClass(self):
00049
00050 self.listener = tf.TransformListener()
00051
00052 self.larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
00053 self.rarm = actionlib.SimpleActionClient("/rarm_controller/joint_trajectory_action", JointTrajectoryAction)
00054 self.torso = actionlib.SimpleActionClient("/torso_controller/joint_trajectory_action", JointTrajectoryAction)
00055 self.head = actionlib.SimpleActionClient("/head_controller/joint_trajectory_action", JointTrajectoryAction)
00056 self.larm.wait_for_server()
00057 self.rarm.wait_for_server()
00058 self.torso.wait_for_server()
00059 self.head.wait_for_server()
00060
00061 rospy.wait_for_service('/SequencePlayerServiceROSBridge/setTargetPose')
00062 self.set_target_pose = rospy.ServiceProxy('/SequencePlayerServiceROSBridge/setTargetPose', OpenHRP_SequencePlayerService_setTargetPose)
00063 rospy.wait_for_service('/SequencePlayerServiceROSBridge/waitInterpolationOfGroup')
00064 self.wait_interpolation_of_group = rospy.ServiceProxy('/SequencePlayerServiceROSBridge/waitInterpolationOfGroup', OpenHRP_SequencePlayerService_waitInterpolationOfGroup)
00065
00066
00067 def tearDown(self):
00068 self.reset_Pose()
00069 True
00070
00071 def setUp(self):
00072 self.reset_Pose()
00073
00074 def reset_Pose(self):
00075 larm_goal = self.goal_LArm()
00076 larm_goal = self.setup_Positions(larm_goal, [[ 0.0, -40.0, -90.0, 0.0, 0.0, 0.0]])
00077 rarm_goal = self.goal_RArm()
00078 rarm_goal = self.setup_Positions(rarm_goal, [[ 0.0, -40.0, -90.0, 0.0, 0.0, 0.0]])
00079 head_goal = self.goal_Head()
00080 head_goal = self.setup_Positions(head_goal, [[0, 20]])
00081 torso_goal = self.goal_Torso()
00082 torso_goal = self.setup_Positions(torso_goal, [[0]])
00083 self.larm.send_goal(larm_goal)
00084 self.rarm.send_goal(rarm_goal)
00085 self.head.send_goal(head_goal)
00086 self.torso.send_goal(torso_goal)
00087 self.larm.wait_for_result()
00088 self.rarm.wait_for_result()
00089 self.head.wait_for_result()
00090 self.torso.wait_for_result()
00091
00092 def goal_LArm(self):
00093 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00094 goal.trajectory.joint_names.append("LARM_JOINT0")
00095 goal.trajectory.joint_names.append("LARM_JOINT1")
00096 goal.trajectory.joint_names.append("LARM_JOINT2")
00097 goal.trajectory.joint_names.append("LARM_JOINT3")
00098 goal.trajectory.joint_names.append("LARM_JOINT4")
00099 goal.trajectory.joint_names.append("LARM_JOINT5")
00100 return goal
00101
00102 def goal_RArm(self):
00103 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00104 goal.trajectory.joint_names.append("RARM_JOINT0")
00105 goal.trajectory.joint_names.append("RARM_JOINT1")
00106 goal.trajectory.joint_names.append("RARM_JOINT2")
00107 goal.trajectory.joint_names.append("RARM_JOINT3")
00108 goal.trajectory.joint_names.append("RARM_JOINT4")
00109 goal.trajectory.joint_names.append("RARM_JOINT5")
00110 return goal
00111
00112 def goal_Torso(self):
00113 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00114 goal.trajectory.joint_names.append("CHEST_JOINT0")
00115 return goal
00116
00117 def goal_Head(self):
00118 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
00119 goal.trajectory.joint_names.append("HEAD_JOINT0")
00120 goal.trajectory.joint_names.append("HEAD_JOINT1")
00121 return goal
00122
00123 def setup_Positions(self, goal, positions, tm = 1.0):
00124 for p in positions:
00125 point = trajectory_msgs.msg.JointTrajectoryPoint()
00126 point.positions = [ x * math.pi / 180.0 for x in p]
00127 point.time_from_start = rospy.Duration(tm)
00128 goal.trajectory.points.append(point)
00129 tm += tm
00130 return goal
00131
00132 def test_LArmIK(self):
00133
00134
00135
00136
00137 for torso_angle in ([0, -10, 10]):
00138 torso_goal = self.goal_Torso()
00139 torso_goal = self.setup_Positions(torso_goal, [[torso_angle]])
00140 self.torso.send_goal_and_wait(torso_goal)
00141 for p in [[ 0.325, 0.182, 0.074],
00142 [ 0.3, -0.2, 0.074], [ 0.3, -0.1, 0.074], [ 0.3, 0.0, 0.074],
00143 [ 0.3, 0.1, 0.074], [ 0.3, 0.2, 0.074], [ 0.3, 0.3, 0.074],
00144 [ 0.4, -0.1, 0.074], [ 0.4, -0.0, 0.074], [ 0.4, 0.1, 0.074],
00145 [ 0.4, 0.2, 0.074], [ 0.4, 0.3, 0.074], [ 0.4, 0.4, 0.074],
00146 ] :
00147
00148 print "solve ik at p = ", p
00149 ret = self.set_target_pose('LARM', p, [-1.694,-1.571, 1.693], 1.0)
00150 self.assertTrue(ret.operation_return, "ik failed")
00151 ret = self.wait_interpolation_of_group('LARM')
00152 self.assertTrue(ret.operation_return, "wait interpolation failed")
00153
00154 rospy.sleep(1)
00155 now = rospy.Time.now()
00156 self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00157 (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00158 numpy.testing.assert_array_almost_equal(trans, p, decimal=1)
00159 print "current position p = ", trans
00160 print " rot = ", rot
00161 print " = ", quaternion_matrix(rot)[0:3,0:3]
00162
00163
00164
00165
00166
00167
00168
00169
00170 def test_LArm(self):
00171 goal = self.goal_LArm()
00172 goal = self.setup_Positions(goal, [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
00173 [ 25,-139,-157, 45, 0, 0],
00174 [ 0.618, -0.157,-100.0,-15.212, 9.501, -3.188]])
00175 self.larm.send_goal_and_wait(goal)
00176
00177 rospy.sleep(1)
00178 now = rospy.Time.now()
00179 self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00180 (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00181 numpy.testing.assert_array_almost_equal(trans, [0.325493, 0.18236, 0.0744674], decimal=3)
00182 numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00183 numpy.array([[ 0, 0,-1],
00184 [ 0, 1, 0],
00185 [ 1, 0, 0]]), decimal=2)
00186
00187 def test_RArm(self):
00188 goal = self.goal_RArm()
00189 goal = self.setup_Positions(goal, [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
00190 [ 25,-139,-157, 45, 0, 0],
00191 [-0.618, -0.157,-100.0,15.212, 9.501, 3.188]])
00192 self.rarm.send_goal_and_wait(goal)
00193
00194 rospy.sleep(1)
00195 now = rospy.Time.now()
00196 self.listener.waitForTransform("WAIST", "RARM_JOINT5_Link", now, rospy.Duration(1.0))
00197 (trans, rot) = self.listener.lookupTransform("WAIST", "RARM_JOINT5_Link", now)
00198 numpy.testing.assert_array_almost_equal(trans, [0.325493,-0.18236, 0.0744674], decimal=3)
00199 numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00200 numpy.array([[ 0, 0,-1],
00201 [ 0, 1, 0],
00202 [ 1, 0, 0]]), decimal=2)
00203
00204 def test_Torso(self):
00205 goal = self.goal_Torso()
00206 goal = self.setup_Positions(goal, [[ 0.0],
00207 [-162.949],
00208 [ 162.949]])
00209 self.torso.send_goal_and_wait(goal)
00210
00211 rospy.sleep(1)
00212 now = rospy.Time.now()
00213 self.listener.waitForTransform("WAIST", "CHEST_JOINT0_Link", now, rospy.Duration(1.0))
00214 (trans, rot) = self.listener.lookupTransform("WAIST", "CHEST_JOINT0_Link", now)
00215 numpy.testing.assert_array_almost_equal(trans, [0.0, 0.0, 0.0], decimal=3)
00216 numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00217 numpy.array([[-0.956044, -0.293223, 0.0],
00218 [ 0.293223, -0.956044, 0.0],
00219 [ 0.0, 0.0, 1.0]]), decimal=3)
00220
00221 def test_Head(self):
00222 goal = self.goal_Head()
00223 goal = self.setup_Positions(goal, [[ 0.0, 0.0],
00224 [ 70.0, 70.0],
00225 [-70.0,-20.0]])
00226 self.head.send_goal_and_wait(goal)
00227
00228 rospy.sleep(1)
00229 now = rospy.Time.now()
00230 self.listener.waitForTransform("WAIST", "HEAD_JOINT1_Link", now, rospy.Duration(1.0))
00231 (trans, rot) = self.listener.lookupTransform("WAIST", "HEAD_JOINT1_Link", now)
00232 numpy.testing.assert_array_almost_equal(trans, [0.0, 0.0, 0.5695], decimal=3)
00233 numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00234 numpy.array([[0.321394, 0.939693, -0.116978],
00235 [-0.883022, 0.34202, 0.321394],
00236 [0.34202, 0.0, 0.939693]]), decimal=3)
00237
00238 def check_q_data(self, name):
00239 import math
00240 data = []
00241
00242 name = name+".q"
00243 f = open(name, 'w')
00244 for j in self.joint_states:
00245 current_time = j.header.stamp.to_sec();
00246 current_value = [p*180/math.pi for p in j.position]
00247 data.append([current_time, current_value[5]])
00248 f.write(str(current_time)+' '+' '.join([str(i) for i in current_value])+"\n")
00249 f.close()
00250 self.filenames.append(name)
00251 return data
00252
00253 def write_output_to_pdf (self,name):
00254 import os
00255 cmd = "gnuplot -p -e \"set terminal pdf; set output '"+name+"'; plot "
00256 for name in self.filenames:
00257 cmd += "'"+name+"' using 0:7 title '"+name+"' with lines"
00258 if name != self.filenames[-1]:
00259 cmd += ","
00260 cmd += "\""
00261 os.system(cmd)
00262 return cmd
00263
00264 def test_send_goal_and_wait(self):
00265
00266 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -120, 15.2, 9.4, 3.2]], 5))
00267 self.rarm.wait_for_result()
00268 self.joint_states = []
00269 time.sleep(1.0);
00270 tm0 = rospy.Time.now()
00271 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00272 self.rarm.wait_for_result()
00273 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00274 self.rarm.wait_for_result()
00275 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00276 self.rarm.wait_for_result()
00277 tm1 = rospy.Time.now()
00278 data_time = (tm1 - tm0).to_sec()
00279 filename = self.filename_base + "-wait"
00280 data = self.check_q_data(filename)
00281 min_data = min([d[1] for d in data])
00282 max_data = max([d[1] for d in data])
00283 print "check setJointAnglesOfGroup(wait=True), tm = ", data_time, ", ok?", abs(data_time - 15.0) < 0.1
00284 self.assertTrue(abs(data_time - 15.0) < 1.0)
00285 print " min = ", min_data, ", ok?", abs(min_data - -140) < 5
00286 self.assertTrue(abs(min_data - -140) < 5)
00287 print " max = ", max_data, ", ok?", abs(max_data - -100) < 5
00288 self.assertTrue(abs(max_data - -100) < 5)
00289
00290 def test_send_goal_and_nowait(self):
00291 clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00292 for i in range(len(clear_time)):
00293 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -120, 15.2, 9.4, 3.2]], 5))
00294 self.rarm.wait_for_result()
00295 self.joint_states = []
00296 rospy.sleep(1.0)
00297 tm0 = rospy.Time.now()
00298 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00299 rospy.sleep(clear_time[i]);
00300 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00301 self.rarm.wait_for_result()
00302 tm1 = rospy.Time.now()
00303 rospy.sleep(1.0)
00304 filename = self.filename_base + "-no-wait-"+str(clear_time[i])
00305 data = self.check_q_data(filename)
00306 data_time = (tm1 - tm0).to_sec()
00307 min_data = min([d[1] for d in data])
00308 max_data = max([d[1] for d in data])
00309 print "check setJointAnglesOfGroup(wait=False), tm = ", data_time, ", ok?", abs(data_time - (10.0 - (5 - clear_time[i]))) < 1.5, " ", (10.0 - (5 - clear_time[i]))
00310 self.assertTrue(abs(data_time - (10.0 - (5 - clear_time[i]))) < 1.5)
00311 print " min = ", min_data, ", ok?", abs(min_data - (-140+i*40/len(clear_time))) < 20, " ", -140+i*40/len(clear_time)
00312 self.assertTrue(abs(min_data - (-140+i*40/len(clear_time))) < 20)
00313 print " max = ", max_data, ", ok?", abs(max_data - -100) < 5
00314 self.assertTrue(abs(max_data - -100) < 5)
00315
00316 def test_send_goal_and_clear(self):
00317 clear_time = [1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5]
00318 clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00319 for i in range(len(clear_time)):
00320 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00321 self.rarm.wait_for_result()
00322 self.joint_states = []
00323 rospy.sleep(1.0)
00324 tm0 = rospy.Time.now()
00325 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00326 self.rarm.wait_for_result()
00327 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00328 rospy.sleep(clear_time[i])
00329 self.rarm.cancel_goal()
00330 self.rarm.wait_for_result()
00331 tm1 = rospy.Time.now()
00332 rospy.sleep(1.0)
00333 filename = self.filename_base + "-clear-"+str(clear_time[i])
00334 data = self.check_q_data(filename)
00335 data_time = (tm1 - tm0).to_sec()
00336 min_data = min([d[1] for d in data])
00337 max_data = max([d[1] for d in data])
00338 print "check setJointAnglesOfGroup(clear "+str(clear_time[i])+"), tm = ", data_time, ", ok?", abs(data_time - (10 - (5 - clear_time[i]))) < 0.5, " ", (10 - (5 - clear_time[i]))
00339 self.assertTrue(abs(data_time - (10 - (5 - clear_time[i]))) < 0.5)
00340 print " min = ", min_data, ", ok?", abs(min_data - (-140+(i+1)*40/len(clear_time))) < 35, " ", -140+(i+1)*40/len(clear_time)
00341 self.assertTrue(abs(min_data - (-140+(i+1)*40/len(clear_time))) < 35)
00342 print " max = ", max_data, ", ok?", abs(max_data - -100) < 5
00343 self.assertTrue(abs(max_data - -100) < 5)
00344
00345
00346 if __name__ == '__main__':
00347 import rostest
00348 rostest.rosrun(PKG, 'test_hronx_ros_bridge', TestHiroROSBridge)
00349
00350
00351
00352