00001 import unittest 00002 import numpy as np 00003 import ros_numpy 00004 00005 from tf import transformations 00006 00007 from geometry_msgs.msg import Vector3, Quaternion, Transform, Point, Pose 00008 00009 class TestGeometry(unittest.TestCase): 00010 def test_point(self): 00011 p = Point(1, 2, 3) 00012 00013 p_arr = ros_numpy.numpify(p) 00014 np.testing.assert_array_equal(p_arr, [1, 2, 3]) 00015 00016 p_arrh = ros_numpy.numpify(p, hom=True) 00017 np.testing.assert_array_equal(p_arrh, [1, 2, 3, 1]) 00018 00019 self.assertEqual(p, ros_numpy.msgify(Point, p_arr)) 00020 self.assertEqual(p, ros_numpy.msgify(Point, p_arrh)) 00021 self.assertEqual(p, ros_numpy.msgify(Point, p_arrh * 2)) 00022 00023 def test_vector3(self): 00024 v = Vector3(1, 2, 3) 00025 00026 v_arr = ros_numpy.numpify(v) 00027 np.testing.assert_array_equal(v_arr, [1, 2, 3]) 00028 00029 v_arrh = ros_numpy.numpify(v, hom=True) 00030 np.testing.assert_array_equal(v_arrh, [1, 2, 3, 0]) 00031 00032 self.assertEqual(v, ros_numpy.msgify(Vector3, v_arr)) 00033 self.assertEqual(v, ros_numpy.msgify(Vector3, v_arrh)) 00034 00035 with self.assertRaises(AssertionError): 00036 ros_numpy.msgify(Vector3, np.array([0, 0, 0, 1])) 00037 00038 def test_transform(self): 00039 t = Transform( 00040 translation=Vector3(1, 2, 3), 00041 rotation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0)) 00042 ) 00043 00044 t_mat = ros_numpy.numpify(t) 00045 00046 np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0]) 00047 00048 msg = ros_numpy.msgify(Transform, t_mat) 00049 00050 np.testing.assert_allclose(msg.translation.x, t.translation.x) 00051 np.testing.assert_allclose(msg.translation.y, t.translation.y) 00052 np.testing.assert_allclose(msg.translation.z, t.translation.z) 00053 np.testing.assert_allclose(msg.rotation.x, t.rotation.x) 00054 np.testing.assert_allclose(msg.rotation.y, t.rotation.y) 00055 np.testing.assert_allclose(msg.rotation.z, t.rotation.z) 00056 np.testing.assert_allclose(msg.rotation.w, t.rotation.w) 00057 00058 def test_pose(self): 00059 t = Pose( 00060 position=Point(1.0, 2.0, 3.0), 00061 orientation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0)) 00062 ) 00063 00064 t_mat = ros_numpy.numpify(t) 00065 00066 np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0]) 00067 00068 msg = ros_numpy.msgify(Pose, t_mat) 00069 00070 np.testing.assert_allclose(msg.position.x, t.position.x) 00071 np.testing.assert_allclose(msg.position.y, t.position.y) 00072 np.testing.assert_allclose(msg.position.z, t.position.z) 00073 np.testing.assert_allclose(msg.orientation.x, t.orientation.x) 00074 np.testing.assert_allclose(msg.orientation.y, t.orientation.y) 00075 np.testing.assert_allclose(msg.orientation.z, t.orientation.z) 00076 np.testing.assert_allclose(msg.orientation.w, t.orientation.w) 00077 00078 if __name__ == '__main__': 00079 unittest.main()