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 TestHiroROSBridgeController(TestHiroROSBridge):
00007 
00008     def test_tf_and_controller(self):
00009         goal = self.goal_RArm()
00010         for av in [[-0.6, 0, -100, 15.2, 9.4, 3.2]]: #[  25,-139,-157,  45,   0,   0]]:
00011             goal = self.setup_Positions(goal, [av])
00012             self.rarm.send_goal_and_wait(goal)
00013             # check if tf and current link is same
00014             rospy.sleep(1)
00015             now = rospy.Time.now()
00016             self.listener.waitForTransform("WAIST", "RARM_JOINT5_Link", now, rospy.Duration(1.0))
00017             (pos_tf, rot_tf) = self.listener.lookupTransform("WAIST", "RARM_JOINT5_Link", now)
00018             rot_tf = quaternion_matrix(rot_tf)[0:3,0:3]
00019             pos_c = self.robot.getCurrentPosition('RARM_JOINT5','WAIST')
00020             rot_c = self.robot.getCurrentRotation('RARM_JOINT5','WAIST')
00021             numpy.testing.assert_array_almost_equal(pos_tf, pos_c, decimal=3)
00022             numpy.testing.assert_array_almost_equal(rot_tf, rot_c, decimal=2)
00023 
00024         goal = self.goal_LArm()
00025         for av in [[0.6, 0, -100, -15.2, 9.4, -3.2]]: #[  25,-139,-157,  45,   0,   0]]:
00026             goal = self.setup_Positions(goal, [av])
00027             self.larm.send_goal_and_wait(goal)
00028             # check if tf and current link is same
00029             rospy.sleep(1)
00030             now = rospy.Time.now()
00031             self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00032             (pos_tf, rot_tf) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00033             rot_tf = quaternion_matrix(rot_tf)[0:3,0:3]
00034             pos_c = self.robot.getCurrentPosition('LARM_JOINT5','WAIST')
00035             rot_c = self.robot.getCurrentRotation('LARM_JOINT5','WAIST')
00036             numpy.testing.assert_array_almost_equal(pos_tf, pos_c, decimal=3)
00037             numpy.testing.assert_array_almost_equal(rot_tf, rot_c, decimal=2)
00038 
00039 if __name__ == '__main__':
00040     import rostest
00041     rostest.rosrun(PKG, 'test_hronx_ros_bridge_controller', TestHiroROSBridgeController)
00042 
00043 
00044