00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import rospy
00038 from tf.transformations import quaternion_matrix, euler_from_matrix
00039 import time
00040
00041 from hironx_ros_bridge.testutil.test_rosbridge import TestHiroROSBridge
00042
00043 PKG = 'hironx_ros_bridge'
00044
00045
00046 class TestHiroROSBridgeSend(TestHiroROSBridge):
00047
00048 def test_send_goal_and_wait(self):
00049
00050 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -120, 15.2, 9.4, 3.2]], 5))
00051 self.rarm.wait_for_result()
00052 self.joint_states = []
00053 time.sleep(1.0);
00054 tm0 = rospy.Time.now()
00055 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00056 self.rarm.wait_for_result()
00057 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00058 self.rarm.wait_for_result()
00059 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00060 self.rarm.wait_for_result()
00061 tm1 = rospy.Time.now()
00062 data_time = (tm1 - tm0).to_sec()
00063 filename = self.filename_base + "-wait"
00064 data = self.check_q_data(filename)
00065 min_data = min([d[1] for d in data])
00066 max_data = max([d[1] for d in data])
00067 print "check setJointAnglesOfGroup(wait=True), tm = ", data_time, ", ok?", abs(data_time - 15.0) < 0.1
00068 self.assertTrue(abs(data_time - 15.0) < 1.0)
00069 print " min = ", min_data, ", ok?", abs(min_data - -140) < 5
00070 self.assertTrue(abs(min_data - -140) < 5)
00071 print " max = ", max_data, ", ok?", abs(max_data - -100) < 5
00072 self.assertTrue(abs(max_data - -100) < 5)
00073
00074 def test_send_goal_and_nowait(self):
00075 clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00076 clear_time = [4.5, 3.0, 1.0]
00077 for i in range(len(clear_time)):
00078 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -120, 15.2, 9.4, 3.2]], 5))
00079 self.rarm.wait_for_result()
00080 self.joint_states = []
00081 rospy.sleep(1.0)
00082 tm0 = rospy.Time.now()
00083 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00084 rospy.sleep(clear_time[i]);
00085 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00086 self.rarm.wait_for_result()
00087 tm1 = rospy.Time.now()
00088 rospy.sleep(1.0)
00089 filename = self.filename_base + "-no-wait-"+str(clear_time[i])
00090 data = self.check_q_data(filename)
00091 data_time = (tm1 - tm0).to_sec()
00092 min_data = min([d[1] for d in data])
00093 max_data = max([d[1] for d in data])
00094 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]))
00095 self.assertTrue(abs(data_time - (10.0 - (5 - clear_time[i]))) < 1.5)
00096 print " min = ", min_data, ", ok?", abs(min_data - (-140+i*40/len(clear_time))) < 20, " ", -140+i*40/len(clear_time)
00097 self.assertTrue(abs(min_data - (-140+i*40/len(clear_time))) < 20)
00098 print " max = ", max_data, ", ok?", abs(max_data - -100) < 5
00099 self.assertTrue(abs(max_data - -100) < 5)
00100
00101 def test_send_goal_and_clear(self):
00102 clear_time = [1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5]
00103 clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00104 clear_time = [4.5, 3.0, 1.0]
00105 for i in range(len(clear_time)):
00106 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00107 self.rarm.wait_for_result()
00108 self.joint_states = []
00109 rospy.sleep(1.0)
00110 tm0 = rospy.Time.now()
00111 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00112 self.rarm.wait_for_result()
00113 self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00114 rospy.sleep(clear_time[i])
00115 self.rarm.cancel_goal()
00116 self.rarm.wait_for_result()
00117 tm1 = rospy.Time.now()
00118 rospy.sleep(1.0)
00119 filename = self.filename_base + "-clear-"+str(clear_time[i])
00120 data = self.check_q_data(filename)
00121 data_time = (tm1 - tm0).to_sec()
00122 min_data = min([d[1] for d in data])
00123 max_data = max([d[1] for d in data])
00124 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]))
00125 self.assertTrue(abs(data_time - (10 - (5 - clear_time[i]))) < 0.5)
00126 print " min = ", min_data, ", ok?", abs(min_data - (-140+(i+1)*40/len(clear_time))) < 35, " ", -140+(i+1)*40/len(clear_time)
00127 self.assertTrue(abs(min_data - (-140+(i+1)*40/len(clear_time))) < 35)
00128 print " max = ", max_data, ", ok?", abs(max_data - -100) < 5
00129 self.assertTrue(abs(max_data - -100) < 5)
00130
00131 if __name__ == '__main__':
00132 import rostest
00133 rostest.rosrun(PKG, 'test_hronx_ros_bridge_send', TestHiroROSBridgeSend)
00134
00135
00136