00001
00002
00003 from __future__ import print_function
00004
00005 import unittest
00006 import struct
00007 import tf2_sensor_msgs
00008 from sensor_msgs import point_cloud2
00009 from sensor_msgs.msg import PointField
00010 from tf2_ros import TransformStamped
00011 import copy
00012
00013
00014 class PointCloudConversions(unittest.TestCase):
00015 def setUp(self):
00016 self.point_cloud_in = point_cloud2.PointCloud2()
00017 self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
00018 PointField('y', 4, PointField.FLOAT32, 1),
00019 PointField('z', 8, PointField.FLOAT32, 1)]
00020
00021 self.point_cloud_in.point_step = 4 * 3
00022 self.point_cloud_in.height = 1
00023
00024 self.point_cloud_in.width = 2
00025 self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
00026
00027 points = [1, 2, 0, 10, 20, 30]
00028 self.point_cloud_in.data = struct.pack('%sf' % len(points), *points)
00029
00030
00031 self.transform_translate_xyz_300 = TransformStamped()
00032 self.transform_translate_xyz_300.transform.translation.x = 300
00033 self.transform_translate_xyz_300.transform.translation.y = 300
00034 self.transform_translate_xyz_300.transform.translation.z = 300
00035 self.transform_translate_xyz_300.transform.rotation.w = 1
00036
00037 assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)])
00038
00039 def test_simple_transform(self):
00040 old_data = copy.deepcopy(self.point_cloud_in.data)
00041 point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
00042
00043 k = 300
00044 expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)]
00045 new_points = list(point_cloud2.read_points(point_cloud_transformed))
00046 print("new_points are %s" % new_points)
00047 assert(expected_coordinates == new_points)
00048 assert(old_data == self.point_cloud_in.data)
00049
00050
00051
00052 class PointCloudConversionsMultichannel(unittest.TestCase):
00053 TRANSFORM_OFFSET_DISTANCE = 300
00054
00055 def setUp(self):
00056 self.point_cloud_in = point_cloud2.PointCloud2()
00057 self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
00058 PointField('y', 4, PointField.FLOAT32, 1),
00059 PointField('z', 8, PointField.FLOAT32, 1),
00060 PointField('index', 12, PointField.INT32, 1)]
00061
00062 self.point_cloud_in.point_step = 4 * 4
00063 self.point_cloud_in.height = 1
00064
00065 self.point_cloud_in.width = 2
00066 self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
00067
00068 self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)]
00069 for point in self.points:
00070 self.point_cloud_in.data += struct.pack('3fi', *point)
00071
00072 self.transform_translate_xyz_300 = TransformStamped()
00073 self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
00074 self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
00075 self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
00076 self.transform_translate_xyz_300.transform.rotation.w = 1
00077
00078 assert(list(point_cloud2.read_points(self.point_cloud_in)) == self.points)
00079
00080 def test_simple_transform_multichannel(self):
00081 old_data = copy.deepcopy(self.point_cloud_in.data)
00082 point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
00083
00084 expected_coordinates = []
00085 for point in self.points:
00086 expected_coordinates += [(
00087 point[0] + self.TRANSFORM_OFFSET_DISTANCE,
00088 point[1] + self.TRANSFORM_OFFSET_DISTANCE,
00089 point[2] + self.TRANSFORM_OFFSET_DISTANCE,
00090 point[3]
00091 )]
00092
00093 new_points = list(point_cloud2.read_points(point_cloud_transformed))
00094 print("new_points are %s" % new_points)
00095 assert(expected_coordinates == new_points)
00096 assert(old_data == self.point_cloud_in.data)
00097
00098
00099 if __name__ == '__main__':
00100 import rosunit
00101 rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions)
00102 rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMultichannel)
00103