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


ros_numpy
Author(s): Eric Wieser
autogenerated on Tue Mar 28 2017 04:16:38