00001 import unittest 00002 import numpy as np 00003 00004 import ros_numpy 00005 from sensor_msgs.msg import PointCloud2, PointField, Image 00006 from ros_numpy import numpy_msg 00007 00008 class TestPointClouds(unittest.TestCase): 00009 def makeArray(self, npoints): 00010 points_arr = np.zeros((npoints,), dtype=[ 00011 ('x', np.float32), 00012 ('y', np.float32), 00013 ('z', np.float32), 00014 ('r', np.uint8), 00015 ('g', np.uint8), 00016 ('b', np.uint8)]) 00017 points_arr['x'] = np.random.random((npoints,)) 00018 points_arr['y'] = np.random.random((npoints,)) 00019 points_arr['z'] = np.random.random((npoints,)) 00020 points_arr['r'] = np.floor(np.random.random((npoints,))*255) 00021 points_arr['g'] = 0 00022 points_arr['b'] = 255 00023 00024 return points_arr 00025 00026 def test_convert_dtype(self): 00027 fields = [ 00028 PointField(name='x', offset=0, count=1, datatype=PointField.FLOAT32), 00029 PointField(name='y', offset=4, count=1, datatype=PointField.FLOAT32) 00030 ] 00031 dtype = np.dtype([ 00032 ('x', np.float32), 00033 ('y', np.float32) 00034 ]) 00035 conv_fields = ros_numpy.msgify(PointField, dtype, plural=True) 00036 self.assertSequenceEqual(fields, conv_fields, 'dtype->Pointfield Failed with simple values') 00037 00038 conv_dtype = ros_numpy.numpify(fields, point_step=8) 00039 self.assertSequenceEqual(dtype, conv_dtype, 'dtype->Pointfield Failed with simple values') 00040 00041 def test_convert_dtype_inner(self): 00042 fields = [ 00043 PointField(name='x', offset=0, count=1, datatype=PointField.FLOAT32), 00044 PointField(name='y', offset=4, count=1, datatype=PointField.FLOAT32), 00045 PointField(name='vectors', offset=8, count=3, datatype=PointField.FLOAT32) 00046 ] 00047 00048 dtype = np.dtype([ 00049 ('x', np.float32), 00050 ('y', np.float32), 00051 ('vectors', np.float32, (3,)) 00052 ]) 00053 00054 conv_fields = ros_numpy.msgify(PointField, dtype, plural=True) 00055 self.assertSequenceEqual(fields, conv_fields, 'dtype->Pointfield with inner dimensions') 00056 00057 conv_dtype = ros_numpy.numpify(fields, point_step=8) 00058 self.assertEqual(dtype, conv_dtype, 'Pointfield->dtype with inner dimensions') 00059 00060 00061 def test_roundtrip(self): 00062 00063 points_arr = self.makeArray(100) 00064 cloud_msg = ros_numpy.msgify(PointCloud2, points_arr) 00065 new_points_arr = ros_numpy.numpify(cloud_msg) 00066 00067 np.testing.assert_equal(points_arr, new_points_arr) 00068 00069 def test_roundtrip_numpy(self): 00070 00071 points_arr = self.makeArray(100) 00072 cloud_msg = ros_numpy.msgify(numpy_msg(PointCloud2), points_arr) 00073 new_points_arr = ros_numpy.numpify(cloud_msg) 00074 00075 np.testing.assert_equal(points_arr, new_points_arr) 00076 00077 if __name__ == '__main__': 00078 unittest.main()