|
36 | 36 | from tf2_ros import TransformStamped |
37 | 37 | import copy |
38 | 38 |
|
39 | | -## A sample python unit test |
| 39 | + |
| 40 | +# A sample python unit test |
40 | 41 | class PointCloudConversions(unittest.TestCase): |
41 | 42 | def setUp(self): |
42 | 43 | self.point_cloud_in = point_cloud2.PointCloud2() |
43 | | - self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1), |
44 | | - PointField('y', 4, PointField.FLOAT32, 1), |
45 | | - PointField('z', 8, PointField.FLOAT32, 1)] |
| 44 | + self.point_cloud_in.fields = [ |
| 45 | + PointField('x', 0, PointField.FLOAT32, 1), |
| 46 | + PointField('y', 4, PointField.FLOAT32, 1), |
| 47 | + PointField('z', 8, PointField.FLOAT32, 1)] |
46 | 48 |
|
47 | 49 | self.point_cloud_in.point_step = 4 * 3 |
48 | 50 | self.point_cloud_in.height = 1 |
49 | 51 | # we add two points (with x, y, z to the cloud) |
50 | 52 | self.point_cloud_in.width = 2 |
51 | | - self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width |
| 53 | + self.point_cloud_in.row_step = \ |
| 54 | + self.point_cloud_in.point_step * self.point_cloud_in.width |
52 | 55 |
|
53 | 56 | points = [1, 2, 0, 10, 20, 30] |
54 | 57 | self.point_cloud_in.data = struct.pack('%sf' % len(points), *points) |
55 | 58 |
|
56 | | - |
57 | 59 | self.transform_translate_xyz_300 = TransformStamped() |
58 | 60 | self.transform_translate_xyz_300.transform.translation.x = 300 |
59 | 61 | self.transform_translate_xyz_300.transform.translation.y = 300 |
60 | 62 | self.transform_translate_xyz_300.transform.translation.z = 300 |
61 | | - self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w |
| 63 | + # no rotation so we only set w |
| 64 | + self.transform_translate_xyz_300.transform.rotation.w = 1 |
62 | 65 |
|
63 | | - assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)]) |
| 66 | + assert(list(point_cloud2.read_points(self.point_cloud_in)) == |
| 67 | + [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)]) |
64 | 68 |
|
65 | 69 | def test_simple_transform(self): |
66 | | - old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now |
67 | | - point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300) |
| 70 | + # deepcopy is not required here because we have a str for now |
| 71 | + old_data = copy.deepcopy(self.point_cloud_in.data) |
| 72 | + point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud( |
| 73 | + self.point_cloud_in, self.transform_translate_xyz_300) |
68 | 74 |
|
69 | 75 | k = 300 |
70 | 76 | expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)] |
71 | 77 | new_points = list(point_cloud2.read_points(point_cloud_transformed)) |
72 | 78 | print("new_points are %s" % new_points) |
73 | 79 | assert(expected_coordinates == new_points) |
74 | | - assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud |
| 80 | + # checking no modification in input cloud |
| 81 | + assert(old_data == self.point_cloud_in.data) |
| 82 | + |
75 | 83 |
|
76 | 84 | if __name__ == '__main__': |
77 | 85 | import rosunit |
78 | | - rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions) |
79 | | - |
| 86 | + rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", |
| 87 | + PointCloudConversions) |
0 commit comments