projection_test.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 PKG='laser_geometry'
00004 
00005 import rospy
00006 import sensor_msgs.point_cloud2 as pc2
00007 from sensor_msgs.msg import LaserScan
00008 from laser_geometry import LaserProjection
00009 
00010 import numpy as np
00011 from itertools import product
00012 
00013 import unittest
00014 
00015 PROJECTION_TEST_RANGE_MIN =  0.23
00016 PROJECTION_TEST_RANGE_MAX = 40.00
00017 
00018 class BuildScanException:
00019     pass
00020 
00021 def build_constant_scan(
00022         range_val, intensity_val,
00023         angle_min, angle_max, angle_increment, scan_time):
00024     count = np.uint(np.ceil((angle_max - angle_min) / angle_increment))
00025     if count < 0:
00026         raise BuildScanException
00027 
00028     scan = LaserScan()
00029     scan.header.stamp = rospy.rostime.Time.from_sec(10.10)
00030     scan.header.frame_id = "laser_frame"
00031     scan.angle_min = angle_min
00032     scan.angle_max = angle_max
00033     scan.angle_increment = angle_increment
00034     scan.scan_time = scan_time.to_sec()
00035     scan.range_min = PROJECTION_TEST_RANGE_MIN
00036     scan.range_max = PROJECTION_TEST_RANGE_MAX
00037     scan.ranges = [range_val for _ in range(count)]
00038     scan.intensities = [intensity_val for _ in range(count)]
00039     scan.time_increment = scan_time.to_sec()/count
00040 
00041     return scan
00042 
00043 class ProjectionTest(unittest.TestCase):
00044 
00045     def test_project_laser(self):
00046         tolerance = 6 # decimal places
00047         projector = LaserProjection()
00048 
00049         ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0]
00050         intensities = np.arange(1.0, 6.0).tolist()
00051 
00052         min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0])
00053         max_angles = -min_angles
00054 
00055         angle_increments = np.pi / np.array([180., 360., 720.])
00056 
00057         scan_times = [rospy.Duration(1./i) for i in [40, 20]]
00058 
00059         for range_val, intensity_val, \
00060             angle_min, angle_max, angle_increment, scan_time in \
00061             product(ranges, intensities,
00062                 min_angles, max_angles, angle_increments, scan_times):
00063             try:
00064                 scan = build_constant_scan(
00065                     range_val, intensity_val,
00066                     angle_min, angle_max, angle_increment, scan_time)
00067             except BuildScanException:
00068                 if (angle_max - angle_min)/angle_increment > 0:
00069                     self.fail()
00070 
00071             cloud_out = projector.projectLaser(scan, -1.0,
00072                     LaserProjection.ChannelOption.INDEX)
00073             self.assertEquals(len(cloud_out.fields), 4,
00074                     "PointCloud2 with channel INDEX: fields size != 4")
00075 
00076             cloud_out = projector.projectLaser(scan, -1.0,
00077                     LaserProjection.ChannelOption.INTENSITY)
00078             self.assertEquals(len(cloud_out.fields), 4,
00079                     "PointCloud2 with channel INDEX: fields size != 4")
00080 
00081             cloud_out = projector.projectLaser(scan, -1.0)
00082             self.assertEquals(len(cloud_out.fields), 5,
00083                     "PointCloud2 with channel INDEX: fields size != 5")
00084             cloud_out = projector.projectLaser(scan, -1.0,
00085                     LaserProjection.ChannelOption.INTENSITY |
00086                     LaserProjection.ChannelOption.INDEX)
00087             self.assertEquals(len(cloud_out.fields), 5,
00088                     "PointCloud2 with channel INDEX: fields size != 5")
00089 
00090             cloud_out = projector.projectLaser(scan, -1.0,
00091                     LaserProjection.ChannelOption.INTENSITY |
00092                     LaserProjection.ChannelOption.INDEX |
00093                     LaserProjection.ChannelOption.DISTANCE)
00094             self.assertEquals(len(cloud_out.fields), 6,
00095                     "PointCloud2 with channel INDEX: fields size != 6")
00096 
00097             cloud_out = projector.projectLaser(scan, -1.0,
00098                     LaserProjection.ChannelOption.INTENSITY |
00099                     LaserProjection.ChannelOption.INDEX |
00100                     LaserProjection.ChannelOption.DISTANCE |
00101                     LaserProjection.ChannelOption.TIMESTAMP)
00102             self.assertEquals(len(cloud_out.fields), 7,
00103                     "PointCloud2 with channel INDEX: fields size != 7")
00104 
00105             valid_points = 0
00106             for i in range(len(scan.ranges)):
00107                 ri = scan.ranges[i]
00108                 if (PROJECTION_TEST_RANGE_MIN <= ri and
00109                     ri <= PROJECTION_TEST_RANGE_MAX):
00110                     valid_points += 1
00111 
00112             self.assertEqual(valid_points, cloud_out.width,
00113                     "Valid points != PointCloud2 width")
00114 
00115             idx_x = idx_y = idx_z = 0
00116             idx_intensity = idx_index = 0
00117             idx_distance = idx_stamps = 0
00118 
00119             i = 0
00120             for f in cloud_out.fields:
00121                 if f.name == "x":
00122                     idx_x = i
00123                 elif f.name == "y":
00124                     idx_y = i
00125                 elif f.name == "z":
00126                     idx_z = i
00127                 elif f.name == "intensity":
00128                     idx_intensity = i
00129                 elif f.name == "index":
00130                     idx_index = i
00131                 elif f.name == "distances":
00132                     idx_distance = i
00133                 elif f.name == "stamps":
00134                     idx_stamps = i
00135                 i += 1
00136 
00137             i = 0
00138             for point in pc2.read_points(cloud_out):
00139                 ri = scan.ranges[i]
00140                 ai = scan.angle_min + i * scan.angle_increment
00141 
00142                 self.assertAlmostEqual(point[idx_x], ri * np.cos(ai),
00143                         tolerance, "x not equal")
00144                 self.assertAlmostEqual(point[idx_y], ri * np.sin(ai),
00145                         tolerance, "y not equal")
00146                 self.assertAlmostEqual(point[idx_z], 0,
00147                         tolerance, "z not equal")
00148                 self.assertAlmostEqual(point[idx_intensity],
00149                         scan.intensities[i],
00150                         tolerance, "Intensity not equal")
00151                 self.assertAlmostEqual(point[idx_index], i,
00152                         tolerance, "Index not equal")
00153                 self.assertAlmostEqual(point[idx_distance], ri,
00154                         tolerance, "Distance not equal")
00155                 self.assertAlmostEqual(point[idx_stamps],
00156                         i * scan.time_increment,
00157                         tolerance, "Timestamp not equal")
00158                 i += 1
00159 
00160 
00161 if __name__ == '__main__':
00162     import rosunit
00163     rosunit.unitrun(PKG, 'projection_test', ProjectionTest)


laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Mon Jul 24 2017 02:35:42