diff --git a/CMakeLists.txt b/CMakeLists.txt
index cf6a070..25d0dd8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,6 +7,7 @@ if(NOT CMAKE_CXX_STANDARD)
endif()
find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_pytest REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
@@ -14,8 +15,7 @@ find_package(tf2 REQUIRED)
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)
-# TODO(dhood): enable python support once ported to ROS 2
-# catkin_python_setup()
+ament_python_install_package(laser_geometry PACKAGE_DIR src/laser_geometry)
add_library(laser_geometry SHARED src/laser_geometry.cpp)
target_include_directories(laser_geometry
@@ -79,6 +79,11 @@ if(BUILD_TESTING)
if(TARGET projection_test)
target_link_libraries(projection_test laser_geometry)
endif()
+
+ ament_add_pytest_test(projection test/projection_test.py
+ TIMEOUT 120
+ WERROR ON
+ )
endif()
ament_package()
diff --git a/package.xml b/package.xml
index 1de752c..2b4a48d 100644
--- a/package.xml
+++ b/package.xml
@@ -24,6 +24,7 @@
http://ros.org/wiki/laser_geometry
ament_cmake
+ ament_cmake_pytest
eigen3_cmake_module
eigen3_cmake_module
@@ -47,6 +48,8 @@
ament_cmake_gmock
ament_cmake_lint_cmake
ament_cmake_uncrustify
+ rclpy
+ sensor_msgs_py
ament_cmake
diff --git a/setup.py b/setup.py
deleted file mode 100644
index 44edf6d..0000000
--- a/setup.py
+++ /dev/null
@@ -1,11 +0,0 @@
-#!/usr/bin/env python
-
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-d = generate_distutils_setup(
- packages=['laser_geometry'],
- package_dir={'': 'src'}
-)
-
-setup(**d)
diff --git a/src/laser_geometry/laser_geometry.py b/src/laser_geometry/laser_geometry.py
index af2452e..227e1ff 100644
--- a/src/laser_geometry/laser_geometry.py
+++ b/src/laser_geometry/laser_geometry.py
@@ -27,9 +27,10 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
"""
-import rospy
+import rclpy
+import rclpy.logging
from sensor_msgs.msg import PointCloud2
-import sensor_msgs.point_cloud2 as pc2
+import sensor_msgs_py.point_cloud2 as pc2
import numpy as np
@@ -107,7 +108,8 @@ class LaserProjection:
if (self.__cos_sin_map.shape[1] != N or
self.__angle_min != scan_in.angle_min or
self.__angle_max != scan_in.angle_max):
- rospy.logdebug("No precomputed map given. Computing one.")
+ rclpy.logging.get_logger("project_laser").debug(
+ "No precomputed map given. Computing one.")
self.__angle_min = scan_in.angle_min
self.__angle_max = scan_in.angle_max
diff --git a/test/projection_test.py b/test/projection_test.py
index e9da60c..0b16d39 100755
--- a/test/projection_test.py
+++ b/test/projection_test.py
@@ -1,21 +1,21 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
-PKG='laser_geometry'
-
-import rospy
-import sensor_msgs.point_cloud2 as pc2
+import rclpy
+import rclpy.duration
+import rclpy.time
+import sensor_msgs_py.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
+import pytest
PROJECTION_TEST_RANGE_MIN = 0.23
PROJECTION_TEST_RANGE_MAX = 40.00
-class BuildScanException:
+class BuildScanException(Exception):
pass
def build_constant_scan(
@@ -26,138 +26,131 @@ def build_constant_scan(
raise BuildScanException
scan = LaserScan()
- scan.header.stamp = rospy.rostime.Time.from_sec(10.10)
+ scan.header.stamp = rclpy.time.Time(seconds=10.10).to_msg()
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.scan_time = scan_time.nanoseconds / 1e9
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
+ scan.time_increment = scan_time.nanoseconds / 1e9 / count
return scan
-class ProjectionTest(unittest.TestCase):
+def test_project_laser():
+ tolerance = 6 # decimal places
+ projector = LaserProjection()
- 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()
- 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
- 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.])
- angle_increments = np.pi / np.array([180., 360., 720.])
+ scan_times = [rclpy.duration.Duration(seconds=1./i) for i in [40, 20]]
- 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:
+ assert (angle_max - angle_min)/angle_increment <= 0
- 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)
+ assert len(cloud_out.fields) == 4, \
+ "PointCloud2 with channel INDEX: fields size != 4"
- 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)
+ assert 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)
+ assert 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)
+ assert len(cloud_out.fields) == 5, \
+ "PointCloud2 with channel INDEX: fields size != 5"
- 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)
+ assert 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)
- 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)
+ assert len(cloud_out.fields) == 7, \
+ "PointCloud2 with channel INDEX: fields size != 7"
- 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
- 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
+ assert valid_points == cloud_out.width, \
+ "Valid points != PointCloud2 width"
- 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
- 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 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
+ i = 0
+ for point in pc2.read_points(cloud_out):
+ ri = scan.ranges[i]
+ ai = scan.angle_min + i * scan.angle_increment
+ assert point[idx_x] == pytest.approx(ri * np.cos(ai),
+ abs=tolerance), "x not equal"
+ assert point[idx_y] == pytest.approx(ri * np.sin(ai),
+ tolerance), "y not equal"
+ assert point[idx_z] == pytest.approx(0, tolerance), "z not equal"
+ assert point[idx_intensity] == pytest.approx(scan.intensities[i],
+ tolerance), "Intensity not equal"
+ assert point[idx_index] == pytest.approx(i, tolerance), \
+ "Index not equal"
+ assert point[idx_distance] == pytest.approx(ri, tolerance), \
+ "Distance not equal"
+ assert point[idx_stamps] == pytest.approx(
+ i * scan.time_increment, tolerance), "Timestamp not equal"
+ i += 1
if __name__ == '__main__':
- import rosunit
- rosunit.unitrun(PKG, 'projection_test', ProjectionTest)
+ pytest.main()
+