projection_test.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 PKG='laser_geometry'
4 
5 import rospy
6 import sensor_msgs.point_cloud2 as pc2
7 from sensor_msgs.msg import LaserScan
8 from laser_geometry import LaserProjection
9 
10 import numpy as np
11 from itertools import product
12 
13 import unittest
14 
15 PROJECTION_TEST_RANGE_MIN = 0.23
16 PROJECTION_TEST_RANGE_MAX = 40.00
17 
18 class BuildScanException:
19  pass
20 
21 def build_constant_scan(
22  range_val, intensity_val,
23  angle_min, angle_max, angle_increment, scan_time):
24  count = np.uint(np.ceil((angle_max - angle_min) / angle_increment))
25  if count < 0:
26  raise BuildScanException
27 
28  scan = LaserScan()
29  scan.header.stamp = rospy.rostime.Time.from_sec(10.10)
30  scan.header.frame_id = "laser_frame"
31  scan.angle_min = angle_min
32  scan.angle_max = angle_max
33  scan.angle_increment = angle_increment
34  scan.scan_time = scan_time.to_sec()
35  scan.range_min = PROJECTION_TEST_RANGE_MIN
36  scan.range_max = PROJECTION_TEST_RANGE_MAX
37  scan.ranges = [range_val for _ in range(count)]
38  scan.intensities = [intensity_val for _ in range(count)]
39  scan.time_increment = scan_time.to_sec()/count
40 
41  return scan
42 
43 class ProjectionTest(unittest.TestCase):
44 
45  def test_project_laser(self):
46  tolerance = 6 # decimal places
47  projector = LaserProjection()
48 
49  ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0]
50  intensities = np.arange(1.0, 6.0).tolist()
51 
52  min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0])
53  max_angles = -min_angles
54 
55  angle_increments = np.pi / np.array([180., 360., 720.])
56 
57  scan_times = [rospy.Duration(1./i) for i in [40, 20]]
58 
59  for range_val, intensity_val, \
60  angle_min, angle_max, angle_increment, scan_time in \
61  product(ranges, intensities,
62  min_angles, max_angles, angle_increments, scan_times):
63  try:
64  scan = build_constant_scan(
65  range_val, intensity_val,
66  angle_min, angle_max, angle_increment, scan_time)
67  except BuildScanException:
68  if (angle_max - angle_min)/angle_increment > 0:
69  self.fail()
70 
71  cloud_out = projector.projectLaser(scan, -1.0,
72  LaserProjection.ChannelOption.INDEX)
73  self.assertEquals(len(cloud_out.fields), 4,
74  "PointCloud2 with channel INDEX: fields size != 4")
75 
76  cloud_out = projector.projectLaser(scan, -1.0,
77  LaserProjection.ChannelOption.INTENSITY)
78  self.assertEquals(len(cloud_out.fields), 4,
79  "PointCloud2 with channel INDEX: fields size != 4")
80 
81  cloud_out = projector.projectLaser(scan, -1.0)
82  self.assertEquals(len(cloud_out.fields), 5,
83  "PointCloud2 with channel INDEX: fields size != 5")
84  cloud_out = projector.projectLaser(scan, -1.0,
85  LaserProjection.ChannelOption.INTENSITY |
86  LaserProjection.ChannelOption.INDEX)
87  self.assertEquals(len(cloud_out.fields), 5,
88  "PointCloud2 with channel INDEX: fields size != 5")
89 
90  cloud_out = projector.projectLaser(scan, -1.0,
91  LaserProjection.ChannelOption.INTENSITY |
92  LaserProjection.ChannelOption.INDEX |
93  LaserProjection.ChannelOption.DISTANCE)
94  self.assertEquals(len(cloud_out.fields), 6,
95  "PointCloud2 with channel INDEX: fields size != 6")
96 
97  cloud_out = projector.projectLaser(scan, -1.0,
98  LaserProjection.ChannelOption.INTENSITY |
99  LaserProjection.ChannelOption.INDEX |
100  LaserProjection.ChannelOption.DISTANCE |
101  LaserProjection.ChannelOption.TIMESTAMP)
102  self.assertEquals(len(cloud_out.fields), 7,
103  "PointCloud2 with channel INDEX: fields size != 7")
104 
105  valid_points = 0
106  for i in range(len(scan.ranges)):
107  ri = scan.ranges[i]
108  if (PROJECTION_TEST_RANGE_MIN <= ri and
109  ri <= PROJECTION_TEST_RANGE_MAX):
110  valid_points += 1
111 
112  self.assertEqual(valid_points, cloud_out.width,
113  "Valid points != PointCloud2 width")
114 
115  idx_x = idx_y = idx_z = 0
116  idx_intensity = idx_index = 0
117  idx_distance = idx_stamps = 0
118 
119  i = 0
120  for f in cloud_out.fields:
121  if f.name == "x":
122  idx_x = i
123  elif f.name == "y":
124  idx_y = i
125  elif f.name == "z":
126  idx_z = i
127  elif f.name == "intensity":
128  idx_intensity = i
129  elif f.name == "index":
130  idx_index = i
131  elif f.name == "distances":
132  idx_distance = i
133  elif f.name == "stamps":
134  idx_stamps = i
135  i += 1
136 
137  i = 0
138  for point in pc2.read_points(cloud_out):
139  ri = scan.ranges[i]
140  ai = scan.angle_min + i * scan.angle_increment
141 
142  self.assertAlmostEqual(point[idx_x], ri * np.cos(ai),
143  tolerance, "x not equal")
144  self.assertAlmostEqual(point[idx_y], ri * np.sin(ai),
145  tolerance, "y not equal")
146  self.assertAlmostEqual(point[idx_z], 0,
147  tolerance, "z not equal")
148  self.assertAlmostEqual(point[idx_intensity],
149  scan.intensities[i],
150  tolerance, "Intensity not equal")
151  self.assertAlmostEqual(point[idx_index], i,
152  tolerance, "Index not equal")
153  self.assertAlmostEqual(point[idx_distance], ri,
154  tolerance, "Distance not equal")
155  self.assertAlmostEqual(point[idx_stamps],
156  i * scan.time_increment,
157  tolerance, "Timestamp not equal")
158  i += 1
159 
160 
161 if __name__ == '__main__':
162  import rosunit
163  rosunit.unitrun(PKG, 'projection_test', ProjectionTest)
laser_geometry::LaserProjection
A Class to Project Laser Scan.
Definition: laser_geometry.h:113
projection_test.BuildScanException
Definition: projection_test.py:18
sensor_msgs::point_cloud2
projection_test.build_constant_scan
def build_constant_scan(range_val, intensity_val, angle_min, angle_max, angle_increment, scan_time)
Definition: projection_test.py:23
projection_test.ProjectionTest
Definition: projection_test.py:43
projection_test.ProjectionTest.test_project_laser
def test_project_laser(self)
Definition: projection_test.py:45

laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu, William Woodall
autogenerated on Sun Feb 7 2021 03:09:58