hironx_ros_bridge: test_hironx_ros_bridge

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 from test_hironx_ros_bridge import *
00005 
00006 class TestHiroROSBridgeSend(TestHiroROSBridge):
00007 
00008     def test_send_goal_and_wait(self):
00009         #self.rarm.send_goal(self.setup_Positions(self.goal_LArm(), [[-0.6, 0, -120, 15.2, 9.4, 3.2]])) this should returns error
00010         self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -120, 15.2, 9.4, 3.2]], 5))
00011         self.rarm.wait_for_result()
00012         self.joint_states = []
00013         time.sleep(1.0);
00014         tm0 = rospy.Time.now()
00015         self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00016         self.rarm.wait_for_result()
00017         self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00018         self.rarm.wait_for_result()
00019         self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00020         self.rarm.wait_for_result()
00021         tm1 = rospy.Time.now()
00022         data_time = (tm1 - tm0).to_sec()
00023         filename = self.filename_base + "-wait"
00024         data = self.check_q_data(filename)
00025         min_data = min([d[1] for d in data])
00026         max_data = max([d[1] for d in data])
00027         print "check setJointAnglesOfGroup(wait=True),  tm = ", data_time, ", ok?", abs(data_time - 15.0) < 0.1
00028         self.assertTrue(abs(data_time - 15.0) < 1.0)
00029         print "                                        min = ", min_data, ", ok?", abs(min_data - -140) < 5
00030         self.assertTrue(abs(min_data - -140) < 5)
00031         print "                                        max = ", max_data, ", ok?", abs(max_data - -100) < 5
00032         self.assertTrue(abs(max_data - -100) < 5)
00033 
00034     def test_send_goal_and_nowait(self):
00035         clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00036         clear_time = [4.5, 3.0, 1.0]
00037         for i in range(len(clear_time)):
00038             self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -120, 15.2, 9.4, 3.2]], 5))
00039             self.rarm.wait_for_result()
00040             self.joint_states = []
00041             rospy.sleep(1.0)
00042             tm0 = rospy.Time.now()
00043             self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00044             rospy.sleep(clear_time[i]);
00045             self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00046             self.rarm.wait_for_result()
00047             tm1 = rospy.Time.now()
00048             rospy.sleep(1.0)
00049             filename = self.filename_base + "-no-wait-"+str(clear_time[i])
00050             data = self.check_q_data(filename)
00051             data_time = (tm1 - tm0).to_sec()
00052             min_data = min([d[1] for d in data])
00053             max_data = max([d[1] for d in data])
00054             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]))
00055             self.assertTrue(abs(data_time - (10.0 - (5 - clear_time[i]))) < 1.5)
00056             print "                                        min = ", min_data, ", ok?", abs(min_data - (-140+i*40/len(clear_time))) < 20, " ", -140+i*40/len(clear_time)
00057             self.assertTrue(abs(min_data - (-140+i*40/len(clear_time))) < 20)
00058             print "                                        max = ", max_data, ", ok?", abs(max_data - -100) < 5
00059             self.assertTrue(abs(max_data - -100) < 5)
00060 
00061     def test_send_goal_and_clear(self):
00062         clear_time = [1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5]
00063         clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00064         clear_time = [4.5, 3.0, 1.0]
00065         for i in range(len(clear_time)):
00066             self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00067             self.rarm.wait_for_result()
00068             self.joint_states = []
00069             rospy.sleep(1.0)
00070             tm0 = rospy.Time.now()
00071             self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -100, 15.2, 9.4, 3.2]], 5))
00072             self.rarm.wait_for_result()
00073             self.rarm.send_goal(self.setup_Positions(self.goal_RArm(), [[-0.6, 0, -140, 15.2, 9.4, 3.2]], 5))
00074             rospy.sleep(clear_time[i])
00075             self.rarm.cancel_goal()
00076             self.rarm.wait_for_result()
00077             tm1 = rospy.Time.now()
00078             rospy.sleep(1.0)
00079             filename = self.filename_base + "-clear-"+str(clear_time[i])
00080             data = self.check_q_data(filename)
00081             data_time = (tm1 - tm0).to_sec()
00082             min_data = min([d[1] for d in data])
00083             max_data = max([d[1] for d in data])
00084             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]))
00085             self.assertTrue(abs(data_time - (10 - (5 - clear_time[i]))) < 0.5)
00086             print "                                        min = ", min_data, ", ok?", abs(min_data - (-140+(i+1)*40/len(clear_time))) < 35, " ", -140+(i+1)*40/len(clear_time)
00087             self.assertTrue(abs(min_data - (-140+(i+1)*40/len(clear_time))) < 35)
00088             print "                                        max = ", max_data, ", ok?", abs(max_data - -100) < 5
00089             self.assertTrue(abs(max_data - -100) < 5)
00090 
00091 if __name__ == '__main__':
00092     import rostest
00093     rostest.rosrun(PKG, 'test_hronx_ros_bridge_send', TestHiroROSBridgeSend)
00094 
00095 
00096