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
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import numpy
00038 import rospy
00039 from tf.transformations import quaternion_matrix, euler_from_matrix
00040 import time
00041
00042 from hironx_ros_bridge.testutil.test_rosbridge import TestHiroROSBridge
00043
00044 PKG = 'hironx_ros_bridge'
00045
00046
00047 class TestHiroROSBridgeController(TestHiroROSBridge):
00048
00049 def test_tf_and_controller(self):
00050 goal = self.goal_RArm()
00051 for av in [[-0.6, 0, -100, 15.2, 9.4, 3.2]]:
00052 goal = self.setup_Positions(goal, [av])
00053 self.rarm.send_goal_and_wait(goal)
00054
00055 rospy.sleep(1)
00056 now = rospy.Time.now()
00057 self.listener.waitForTransform("WAIST", "RARM_JOINT5_Link", now, rospy.Duration(1.0))
00058 (pos_tf, rot_tf) = self.listener.lookupTransform("WAIST", "RARM_JOINT5_Link", now)
00059 rot_tf = quaternion_matrix(rot_tf)[0:3,0:3]
00060 pos_c = self.robot.getCurrentPosition('RARM_JOINT5','WAIST')
00061 rot_c = self.robot.getCurrentRotation('RARM_JOINT5','WAIST')
00062 numpy.testing.assert_array_almost_equal(pos_tf, pos_c, decimal=3)
00063 numpy.testing.assert_array_almost_equal(rot_tf, rot_c, decimal=2)
00064
00065 goal = self.goal_LArm()
00066 for av in [[0.6, 0, -100, -15.2, 9.4, -3.2]]:
00067 goal = self.setup_Positions(goal, [av])
00068 self.larm.send_goal_and_wait(goal)
00069
00070 rospy.sleep(1)
00071 now = rospy.Time.now()
00072 self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00073 (pos_tf, rot_tf) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00074 rot_tf = quaternion_matrix(rot_tf)[0:3,0:3]
00075 pos_c = self.robot.getCurrentPosition('LARM_JOINT5','WAIST')
00076 rot_c = self.robot.getCurrentRotation('LARM_JOINT5','WAIST')
00077 numpy.testing.assert_array_almost_equal(pos_tf, pos_c, decimal=3)
00078 numpy.testing.assert_array_almost_equal(rot_tf, rot_c, decimal=2)
00079
00080 if __name__ == '__main__':
00081 import rostest
00082 rostest.rosrun(PKG, 'test_hronx_ros_bridge_controller', TestHiroROSBridgeController)
00083
00084
00085