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
This commit is contained in:
Jonathan Binney
GitHub
parent
61c04d33ed
commit
af5bdc2874
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
<url>http://ros.org/wiki/laser_geometry</url>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>ament_cmake_pytest</buildtool_depend>
|
||||
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
|
||||
|
||||
<buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
|
||||
|
|
@ -47,6 +48,8 @@
|
|||
<test_depend>ament_cmake_gmock</test_depend>
|
||||
<test_depend>ament_cmake_lint_cmake</test_depend>
|
||||
<test_depend>ament_cmake_uncrustify</test_depend>
|
||||
<test_depend>rclpy</test_depend>
|
||||
<test_depend>sensor_msgs_py</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
|
|
|
|||
11
setup.py
11
setup.py
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user