3 from __future__
import print_function
8 from sensor_msgs
import point_cloud2
9 from sensor_msgs.msg
import PointField
10 from tf2_ros
import TransformStamped
17 self.point_cloud_in.fields = [PointField(
'x', 0, PointField.FLOAT32, 1),
18 PointField(
'y', 4, PointField.FLOAT32, 1),
19 PointField(
'z', 8, PointField.FLOAT32, 1)]
21 self.point_cloud_in.point_step = 4 * 3
22 self.point_cloud_in.height = 1
24 self.point_cloud_in.width = 2
25 self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
27 points = [1, 2, 0, 10, 20, 30]
28 self.point_cloud_in.data = struct.pack(
'%sf' % len(points), *points)
32 self.transform_translate_xyz_300.transform.translation.x = 300
33 self.transform_translate_xyz_300.transform.translation.y = 300
34 self.transform_translate_xyz_300.transform.translation.z = 300
35 self.transform_translate_xyz_300.transform.rotation.w = 1
37 assert(list(point_cloud2.read_points(self.
point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)])
40 old_data = copy.deepcopy(self.point_cloud_in.data)
44 expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)]
45 new_points = list(point_cloud2.read_points(point_cloud_transformed))
46 print(
"new_points are %s" % new_points)
47 assert(expected_coordinates == new_points)
48 assert(old_data == self.point_cloud_in.data)
53 TRANSFORM_OFFSET_DISTANCE = 300
57 self.point_cloud_in.fields = [PointField(
'x', 0, PointField.FLOAT32, 1),
58 PointField(
'y', 4, PointField.FLOAT32, 1),
59 PointField(
'z', 8, PointField.FLOAT32, 1),
60 PointField(
'index', 12, PointField.INT32, 1)]
62 self.point_cloud_in.point_step = 4 * 4
63 self.point_cloud_in.height = 1
65 self.point_cloud_in.width = 2
66 self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
68 self.
points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)]
70 self.point_cloud_in.data += struct.pack(
'3fi', *point)
76 self.transform_translate_xyz_300.transform.rotation.w = 1
81 old_data = copy.deepcopy(self.point_cloud_in.data)
84 expected_coordinates = []
86 expected_coordinates += [(
93 new_points = list(point_cloud2.read_points(point_cloud_transformed))
94 print(
"new_points are %s" % new_points)
95 assert(expected_coordinates == new_points)
96 assert(old_data == self.point_cloud_in.data)
99 if __name__ ==
'__main__':
101 rosunit.unitrun(
"test_tf2_sensor_msgs",
"test_point_cloud_conversion", PointCloudConversions)
102 rosunit.unitrun(
"test_tf2_sensor_msgs",
"test_point_cloud_conversion", PointCloudConversionsMultichannel)
def test_simple_transform_multichannel(self)
transform_translate_xyz_300
def test_simple_transform(self)
A simple unit test for tf2_sensor_msgs.do_transform_cloud (multi channel version) ...
transform_translate_xyz_300
A sample python unit test.
int TRANSFORM_OFFSET_DISTANCE