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
00038
00039 import numpy
00040 import unittest
00041
00042 import time
00043 import rospy
00044 import tf
00045 from tf.transformations import quaternion_from_euler
00046
00047 class TestNxoCalib(unittest.TestCase):
00048
00049 def setUp(self):
00050 rospy.init_node('test_calib')
00051 self.tflistener = tf.TransformListener()
00052
00053 time.sleep(2)
00054 while len(self.tflistener.getFrameStrings()) < 10:
00055 time.sleep(2)
00056 rospy.loginfo(self.tflistener.getFrameStrings())
00057 rospy.loginfo(self.tflistener.getFrameStrings())
00058
00059 def tearDown(self):
00060 True
00061
00062 def test_tf_head_checkerboard_chest(self):
00063 '''
00064 @summary: Check if the translation between 2 frames, head and
00065 the camera mounted on top of the head, fall into the intended
00066 range. See the image if you want to get an idea of these 2
00067 frames: https://cloud.githubusercontent.com/assets/3119480/22644078/8858459c-eca4-11e6-9fb5-4b624d79d499.png
00068 '''
00069 FRAME_CAMERA = "/camera_link"
00070 FRAME_HEAD = "/HEAD_JOINT1_Link"
00071 TRANS_EXPECTED = [0.103, -0.023, 0.171]
00072 QUARTERNION_EXPECTED = [0.002, 0.706, -0.002, 0.708]
00073
00074
00075
00076 rospy.loginfo("Check if {} exists".format(FRAME_HEAD))
00077 self.assertTrue(self.tflistener.lookupTransform(FRAME_HEAD, FRAME_HEAD, rospy.Time(0)), "Frame {} does not exists".format(FRAME_HEAD))
00078 rospy.loginfo("Check if {} exists".format(FRAME_CAMERA))
00079 self.assertTrue(self.tflistener.lookupTransform(FRAME_CAMERA, FRAME_CAMERA, rospy.Time(0)), "Frame {} does not exists".format(FRAME_CAMERA))
00080
00081 return True
00082
00083 t = None
00084 while t == None:
00085 try :
00086 time.sleep(2)
00087 rospy.loginfo("Get common time between {} and {}".format(FRAME_HEAD, FRAME_CAMERA))
00088 t = self.tflistener.getLatestCommonTime(FRAME_HEAD, FRAME_CAMERA)
00089 rospy.loginfo(t)
00090 except:
00091 pass
00092
00093 pos, quaternion = self.tflistener.lookupTransform(FRAME_HEAD, FRAME_CAMERA, t)
00094 numpy.testing.assert_almost_equal(pos, TRANS_EXPECTED, 2)
00095 numpy.testing.assert_almost_equal(quaternion, QUARTERNION_EXPECTED, 2)
00096
00097 if __name__ == '__main__':
00098 import rostest
00099 rostest.rosrun('nextage_calibration', 'test_calib', TestNxoCalib)
00100