00001
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
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)