From af5bdc2874a0352bbc4ef9527d21fd2989ddff66 Mon Sep 17 00:00:00 2001 From: Jonathan Binney Date: Tue, 10 Aug 2021 18:11:23 -0700 Subject: [PATCH] Update python code and tests for ros2 (#80) * Enable projection nose test using ament * Update python package and tests for ros2 * Remove unneeded python setup file * Use pytest instead of nose Nose was outputting xml that xUnit (jenkins plugin) couldn't read. * Fix pytest warnings --- CMakeLists.txt | 9 +- package.xml | 3 + setup.py | 11 -- src/laser_geometry/laser_geometry.py | 8 +- test/projection_test.py | 219 +++++++++++++-------------- 5 files changed, 121 insertions(+), 129 deletions(-) delete mode 100644 setup.py 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() +