Go to the documentation of this file.00001
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)