45 from tf.transformations
import quaternion_from_euler
50 rospy.init_node(
'test_calib')
54 while len(self.tflistener.getFrameStrings()) < 10:
56 rospy.loginfo(self.tflistener.getFrameStrings())
57 rospy.loginfo(self.tflistener.getFrameStrings())
64 @summary: Check if the translation between 2 frames, head and 65 the camera mounted on top of the head, fall into the intended 66 range. See the image if you want to get an idea of these 2 67 frames: https://cloud.githubusercontent.com/assets/3119480/22644078/8858459c-eca4-11e6-9fb5-4b624d79d499.png 69 FRAME_CAMERA =
"/camera_link" 70 FRAME_HEAD =
"/HEAD_JOINT1_Link" 71 TRANS_EXPECTED = [0.103, -0.023, 0.171]
72 QUARTERNION_EXPECTED = [0.002, 0.706, -0.002, 0.708]
76 rospy.loginfo(
"Check if {} exists".format(FRAME_HEAD))
77 self.assertTrue(self.tflistener.lookupTransform(FRAME_HEAD, FRAME_HEAD, rospy.Time(0)),
"Frame {} does not exists".format(FRAME_HEAD))
78 rospy.loginfo(
"Check if {} exists".format(FRAME_CAMERA))
79 self.assertTrue(self.tflistener.lookupTransform(FRAME_CAMERA, FRAME_CAMERA, rospy.Time(0)),
"Frame {} does not exists".format(FRAME_CAMERA))
87 rospy.loginfo(
"Get common time between {} and {}".format(FRAME_HEAD, FRAME_CAMERA))
88 t = self.tflistener.getLatestCommonTime(FRAME_HEAD, FRAME_CAMERA)
93 pos, quaternion = self.tflistener.lookupTransform(FRAME_HEAD, FRAME_CAMERA, t)
94 numpy.testing.assert_almost_equal(pos, TRANS_EXPECTED, 2)
95 numpy.testing.assert_almost_equal(quaternion, QUARTERNION_EXPECTED, 2)
97 if __name__ ==
'__main__':
99 rostest.rosrun(
'nextage_calibration',
'test_calib', TestNxoCalib)
def test_tf_head_checkerboard_chest(self)