test_tf2_sensor_msgs.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from __future__ import print_function
4 
5 import unittest
6 import struct
7 import tf2_sensor_msgs
8 from sensor_msgs import point_cloud2
9 from sensor_msgs.msg import PointField
10 from tf2_ros import TransformStamped
11 import copy
12 
13 ## A sample python unit test
14 class PointCloudConversions(unittest.TestCase):
15  def setUp(self):
16  self.point_cloud_in = point_cloud2.PointCloud2()
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)]
20 
21  self.point_cloud_in.point_step = 4 * 3
22  self.point_cloud_in.height = 1
23  # we add two points (with x, y, z to the cloud)
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
26 
27  points = [1, 2, 0, 10, 20, 30]
28  self.point_cloud_in.data = struct.pack('%sf' % len(points), *points)
29 
30 
31  self.transform_translate_xyz_300 = TransformStamped()
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 # no rotation so we only set w
36 
37  assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)])
38 
40  old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now
41  point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
42 
43  k = 300
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) # checking no modification in input cloud
49 
50 
51 ## A simple unit test for tf2_sensor_msgs.do_transform_cloud (multi channel version)
52 class PointCloudConversionsMultichannel(unittest.TestCase):
53  TRANSFORM_OFFSET_DISTANCE = 300
54 
55  def setUp(self):
56  self.point_cloud_in = point_cloud2.PointCloud2()
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)]
61 
62  self.point_cloud_in.point_step = 4 * 4
63  self.point_cloud_in.height = 1
64  # we add two points (with x, y, z to the cloud)
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
67 
68  self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)]
69  for point in self.points:
70  self.point_cloud_in.data += struct.pack('3fi', *point)
71 
72  self.transform_translate_xyz_300 = TransformStamped()
73  self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
74  self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
75  self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
76  self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
77 
78  assert(list(point_cloud2.read_points(self.point_cloud_in)) == self.points)
79 
81  old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now
82  point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
83 
84  expected_coordinates = []
85  for point in self.points:
86  expected_coordinates += [(
87  point[0] + self.TRANSFORM_OFFSET_DISTANCE,
88  point[1] + self.TRANSFORM_OFFSET_DISTANCE,
89  point[2] + self.TRANSFORM_OFFSET_DISTANCE,
90  point[3] # index channel must be kept same
91  )]
92 
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) # checking no modification in input cloud
97 
98 
99 if __name__ == '__main__':
100  import rosunit
101  rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions)
102  rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMultichannel)
103 
A simple unit test for tf2_sensor_msgs.do_transform_cloud (multi channel version) ...


tf2_sensor_msgs
Author(s): Vincent Rabaud
autogenerated on Fri Oct 16 2020 19:09:07