diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1260dc0..67a2739 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -34,6 +34,8 @@ target_link_libraries(laser_geometry ${Boost_LIBRARIES} ${tf_LIBRARIES})
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(projection_test test/projection_test.cpp)
target_link_libraries(projection_test laser_geometry)
+
+ catkin_add_nosetests(test/projection_test.py)
endif()
install(TARGETS laser_geometry
diff --git a/package.xml b/package.xml
index d836d0f..4b631cc 100644
--- a/package.xml
+++ b/package.xml
@@ -33,4 +33,5 @@
sensor_msgs
tf
+ unittest
diff --git a/test/projection_test.py b/test/projection_test.py
new file mode 100755
index 0000000..a6102c0
--- /dev/null
+++ b/test/projection_test.py
@@ -0,0 +1,170 @@
+#!/usr/bin/env python
+
+PKG='laser_geometry'
+
+import rospy
+import sensor_msgs.point_cloud2 as pc2
+from sensor_msgs.msg import LaserScan
+from laser_geometry import LaserProjection
+
+import numpy as np
+from itertools import product
+
+import unittest
+
+PROJECTION_TEST_RANGE_MIN = 0.23
+PROJECTION_TEST_RANGE_MAX = 40.00
+
+class BuildScanException:
+ pass
+
+def build_constant_scan(
+ range_val, intensity_val,
+ angle_min, angle_max, angle_increment, scan_time):
+ count = np.uint(np.ceil((angle_max - angle_min) / angle_increment))
+ if count < 0:
+ raise BuildScanException
+
+ scan = LaserScan()
+ scan.header.stamp = rospy.Time.now()
+ scan.header.frame_id = "laser_frame"
+ scan.angle_min = angle_min
+ scan.angle_max = angle_max
+ scan.angle_increment = angle_increment
+ scan.scan_time = scan_time.to_sec()
+ scan.range_min = PROJECTION_TEST_RANGE_MIN
+ scan.range_max = PROJECTION_TEST_RANGE_MAX
+ scan.ranges = [range_val for _ in range(count)]
+ scan.intensities = [intensity_val for _ in range(count)]
+ scan.time_increment = scan_time.to_sec()/count
+
+ return scan
+
+class ProjectionTest(unittest.TestCase):
+
+ @classmethod
+ def setUpClass(cls):
+ rospy.init_node('projection_test')
+
+ def tearDown(self):
+ pass
+
+ def test_project_laser(self):
+ tolerance = 6 # decimal places
+ projector = LaserProjection()
+
+ ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0]
+ intensities = np.arange(1.0, 6.0).tolist()
+
+ min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0])
+ max_angles = -min_angles
+
+ angle_increments = np.pi / np.array([180., 360., 720.])
+
+ scan_times = [rospy.Duration(1./i) for i in [40, 20]]
+
+ for range_val, intensity_val, \
+ angle_min, angle_max, angle_increment, scan_time in \
+ product(ranges, intensities,
+ min_angles, max_angles, angle_increments, scan_times):
+ try:
+ scan = build_constant_scan(
+ range_val, intensity_val,
+ angle_min, angle_max, angle_increment, scan_time)
+ except BuildScanException:
+ if (angle_max - angle_min)/angle_increment > 0:
+ self.fail()
+
+ cloud_out = projector.projectLaser(scan, -1.0,
+ LaserProjection.ChannelOption.INDEX)
+ self.assertEquals(len(cloud_out.fields), 4,
+ "PointCloud2 with channel INDEX: fields size != 4")
+
+ cloud_out = projector.projectLaser(scan, -1.0,
+ LaserProjection.ChannelOption.INTENSITY)
+ self.assertEquals(len(cloud_out.fields), 4,
+ "PointCloud2 with channel INDEX: fields size != 4")
+
+ cloud_out = projector.projectLaser(scan, -1.0)
+ self.assertEquals(len(cloud_out.fields), 5,
+ "PointCloud2 with channel INDEX: fields size != 5")
+ cloud_out = projector.projectLaser(scan, -1.0,
+ LaserProjection.ChannelOption.INTENSITY |
+ LaserProjection.ChannelOption.INDEX)
+ self.assertEquals(len(cloud_out.fields), 5,
+ "PointCloud2 with channel INDEX: fields size != 5")
+
+ cloud_out = projector.projectLaser(scan, -1.0,
+ LaserProjection.ChannelOption.INTENSITY |
+ LaserProjection.ChannelOption.INDEX |
+ LaserProjection.ChannelOption.DISTANCE)
+ self.assertEquals(len(cloud_out.fields), 6,
+ "PointCloud2 with channel INDEX: fields size != 6")
+
+ cloud_out = projector.projectLaser(scan, -1.0,
+ LaserProjection.ChannelOption.INTENSITY |
+ LaserProjection.ChannelOption.INDEX |
+ LaserProjection.ChannelOption.DISTANCE |
+ LaserProjection.ChannelOption.TIMESTAMP)
+ self.assertEquals(len(cloud_out.fields), 7,
+ "PointCloud2 with channel INDEX: fields size != 7")
+
+ valid_points = 0
+ for i in range(len(scan.ranges)):
+ ri = scan.ranges[i]
+ if (PROJECTION_TEST_RANGE_MIN <= ri and
+ ri <= PROJECTION_TEST_RANGE_MAX):
+ valid_points += 1
+
+ self.assertEqual(valid_points, cloud_out.width,
+ "Valid points != PointCloud2 width")
+
+ idx_x = idx_y = idx_z = 0
+ idx_intensity = idx_index = 0
+ idx_distance = idx_stamps = 0
+
+ i = 0
+ for f in cloud_out.fields:
+ if f.name == "x":
+ idx_x = i
+ elif f.name == "y":
+ idx_y = i
+ elif f.name == "z":
+ idx_z = i
+ elif f.name == "intensity":
+ idx_intensity = i
+ elif f.name == "index":
+ idx_index = i
+ elif f.name == "distances":
+ idx_distance = i
+ elif f.name == "stamps":
+ idx_stamps = i
+ i += 1
+
+ i = 0
+ for point in pc2.read_points(cloud_out):
+ ri = scan.ranges[i]
+ ai = scan.angle_min + i * scan.angle_increment
+
+ self.assertAlmostEqual(point[idx_x], ri * np.cos(ai),
+ tolerance, "x not equal")
+ self.assertAlmostEqual(point[idx_y], ri * np.sin(ai),
+ tolerance, "y not equal")
+ self.assertAlmostEqual(point[idx_z], 0,
+ tolerance, "z not equal")
+ self.assertAlmostEqual(point[idx_intensity],
+ scan.intensities[i],
+ tolerance, "Intensity not equal")
+ self.assertAlmostEqual(point[idx_index], i,
+ tolerance, "Index not equal")
+ self.assertAlmostEqual(point[idx_distance], ri,
+ tolerance, "Distance not equal")
+ self.assertAlmostEqual(point[idx_stamps],
+ i * scan.time_increment,
+ tolerance, "Timestamp not equal")
+ i += 1
+
+
+if __name__ == '__main__':
+ import rosunit
+ rosunit.unitrun(PKG, 'projection_test', ProjectionTest)