camera_pose_calibration: test_camera_pose

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 PKG = 'camera_pose_calibration'
00004 import roslib; roslib.load_manifest(PKG)
00005 import rospy
00006 import tf
00007 import PyKDL
00008 import unittest
00009 import rostest
00010 
00011 
00012 class TestCameraPose(unittest.TestCase):
00013     def test_one(self):
00014         tf_listener = tf.TransformListener()
00015         fr1 = 'camera_a/openni_depth_optical_frame'
00016         fr2 = 'camera_c/prosilica_optical_frame'
00017 
00018         tf_listener.waitForTransform(fr1, fr2, rospy.Time(0), rospy.Duration(400))
00019         ((x, y, z), (rx, ry, rz, rw)) = tf_listener.lookupTransform(fr1, fr2, rospy.Time(0))
00020         self.assertAlmostEqual(x, -0.011, 1)
00021         self.assertAlmostEqual(y, 0.027, 1)
00022         self.assertAlmostEqual(z, 0.048, 1)
00023         self.assertAlmostEqual(rx, 0.022, 1)
00024         self.assertAlmostEqual(ry, 0.007, 1)
00025         self.assertAlmostEqual(rz, -0.006, 1)
00026         self.assertAlmostEqual(rw, 1.00, 1)
00027         
00028 
00029 
00030 if __name__ == "__main__":
00031     rospy.init_node('test_camera_pose_calibration')
00032     rospy.sleep(0.1)
00033     rostest.rosrun(PKG, 'test_camera_pose_calibration', TestCameraPose)