DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update python code and tests for ros2 (#80)

Browse Source 
* 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 2021-08-10 18:11:23 -07:00 committed by GitHub
parent 61c04d33ed
commit af5bdc2874
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 121 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
CMakeLists.txt
View File

@ -7,6 +7,7 @@ if(NOT CMAKE_CXX_STANDARD)
endif() endif()
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(ament_cmake_pytest REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED) find_package(sensor_msgs REQUIRED)
@ -14,8 +15,7 @@ find_package(tf2 REQUIRED)
find_package(eigen3_cmake_module REQUIRED) find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED) find_package(Eigen3 REQUIRED)
# TODO(dhood): enable python support once ported to ROS 2 ament_python_install_package(laser_geometry PACKAGE_DIR src/laser_geometry)
# catkin_python_setup()
add_library(laser_geometry SHARED src/laser_geometry.cpp) add_library(laser_geometry SHARED src/laser_geometry.cpp)
target_include_directories(laser_geometry target_include_directories(laser_geometry
@ -79,6 +79,11 @@ if(BUILD_TESTING)
if(TARGET projection_test) if(TARGET projection_test)
target_link_libraries(projection_test laser_geometry) target_link_libraries(projection_test laser_geometry)
endif() endif()
ament_add_pytest_test(projection test/projection_test.py
TIMEOUT 120
WERROR ON
)
endif() endif()
ament_package() ament_package()

3
package.xml
View File

@ -24,6 +24,7 @@
<url>http://ros.org/wiki/laser_geometry</url> <url>http://ros.org/wiki/laser_geometry</url>
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_pytest</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend> <buildtool_depend>eigen3_cmake_module</buildtool_depend>
<buildtool_export_depend>eigen3_cmake_module</buildtool_export_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_gmock</test_depend>
<test_depend>ament_cmake_lint_cmake</test_depend> <test_depend>ament_cmake_lint_cmake</test_depend>
<test_depend>ament_cmake_uncrustify</test_depend> <test_depend>ament_cmake_uncrustify</test_depend>
<test_depend>rclpy</test_depend>
<test_depend>sensor_msgs_py</test_depend>
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>

11
setup.py
View File

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

8
src/laser_geometry/laser_geometry.py
View File

@ -27,9 +27,10 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. POSSIBILITY OF SUCH DAMAGE.
""" """
import rospy import rclpy
import rclpy.logging
from sensor_msgs.msg import PointCloud2 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 import numpy as np
@ -107,7 +108,8 @@ class LaserProjection:
if (self.__cos_sin_map.shape[1] != N or if (self.__cos_sin_map.shape[1] != N or
self.__angle_min != scan_in.angle_min or self.__angle_min != scan_in.angle_min or
self.__angle_max != scan_in.angle_max): 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_min = scan_in.angle_min
self.__angle_max = scan_in.angle_max self.__angle_max = scan_in.angle_max

219
test/projection_test.py
View File

@ -1,21 +1,21 @@
#!/usr/bin/env python #!/usr/bin/env python3
PKG='laser_geometry' import rclpy
import rclpy.duration
import rospy import rclpy.time
import sensor_msgs.point_cloud2 as pc2 import sensor_msgs_py.point_cloud2 as pc2
from sensor_msgs.msg import LaserScan from sensor_msgs.msg import LaserScan
from laser_geometry import LaserProjection from laser_geometry import LaserProjection
import numpy as np import numpy as np
from itertools import product from itertools import product
import unittest import pytest
PROJECTION_TEST_RANGE_MIN = 0.23 PROJECTION_TEST_RANGE_MIN = 0.23
PROJECTION_TEST_RANGE_MAX = 40.00 PROJECTION_TEST_RANGE_MAX = 40.00
class BuildScanException: class BuildScanException(Exception):
pass pass
def build_constant_scan( def build_constant_scan(
@ -26,138 +26,131 @@ def build_constant_scan(
raise BuildScanException raise BuildScanException
scan = LaserScan() 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.header.frame_id = "laser_frame"
scan.angle_min = angle_min scan.angle_min = angle_min
scan.angle_max = angle_max scan.angle_max = angle_max
scan.angle_increment = angle_increment 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_min = PROJECTION_TEST_RANGE_MIN
scan.range_max = PROJECTION_TEST_RANGE_MAX scan.range_max = PROJECTION_TEST_RANGE_MAX
scan.ranges = [range_val for _ in range(count)] scan.ranges = [range_val for _ in range(count)]
scan.intensities = [intensity_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 return scan
class ProjectionTest(unittest.TestCase): def test_project_laser():
tolerance = 6 # decimal places
projector = LaserProjection()
def test_project_laser(self): ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0]
tolerance = 6 # decimal places intensities = np.arange(1.0, 6.0).tolist()
projector = LaserProjection()
ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0] min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0])
intensities = np.arange(1.0, 6.0).tolist() max_angles = -min_angles
min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0]) angle_increments = np.pi / np.array([180., 360., 720.])
max_angles = -min_angles
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, \ cloud_out = projector.projectLaser(scan, -1.0,
angle_min, angle_max, angle_increment, scan_time in \ LaserProjection.ChannelOption.INDEX)
product(ranges, intensities, assert len(cloud_out.fields) == 4, \
min_angles, max_angles, angle_increments, scan_times): "PointCloud2 with channel INDEX: fields size != 4"
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, cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INDEX) LaserProjection.ChannelOption.INTENSITY)
self.assertEquals(len(cloud_out.fields), 4, assert len(cloud_out.fields) == 4, \
"PointCloud2 with channel INDEX: fields size != 4") "PointCloud2 with channel INDEX: fields size != 4"
cloud_out = projector.projectLaser(scan, -1.0, cloud_out = projector.projectLaser(scan, -1.0)
LaserProjection.ChannelOption.INTENSITY) assert len(cloud_out.fields) == 5, \
self.assertEquals(len(cloud_out.fields), 4, "PointCloud2 with channel INDEX: fields size != 5"
"PointCloud2 with channel INDEX: fields size != 4") 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) cloud_out = projector.projectLaser(scan, -1.0,
self.assertEquals(len(cloud_out.fields), 5, LaserProjection.ChannelOption.INTENSITY |
"PointCloud2 with channel INDEX: fields size != 5") LaserProjection.ChannelOption.INDEX |
cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.DISTANCE)
LaserProjection.ChannelOption.INTENSITY | assert len(cloud_out.fields) == 6, \
LaserProjection.ChannelOption.INDEX) "PointCloud2 with channel INDEX: fields size != 6"
self.assertEquals(len(cloud_out.fields), 5,
"PointCloud2 with channel INDEX: fields size != 5")
cloud_out = projector.projectLaser(scan, -1.0, cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INTENSITY |
LaserProjection.ChannelOption.INDEX | LaserProjection.ChannelOption.INDEX |
LaserProjection.ChannelOption.DISTANCE) LaserProjection.ChannelOption.DISTANCE |
self.assertEquals(len(cloud_out.fields), 6, LaserProjection.ChannelOption.TIMESTAMP)
"PointCloud2 with channel INDEX: fields size != 6") assert len(cloud_out.fields) == 7, \
"PointCloud2 with channel INDEX: fields size != 7"
cloud_out = projector.projectLaser(scan, -1.0, valid_points = 0
LaserProjection.ChannelOption.INTENSITY | for i in range(len(scan.ranges)):
LaserProjection.ChannelOption.INDEX | ri = scan.ranges[i]
LaserProjection.ChannelOption.DISTANCE | if (PROJECTION_TEST_RANGE_MIN <= ri and
LaserProjection.ChannelOption.TIMESTAMP) ri <= PROJECTION_TEST_RANGE_MAX):
self.assertEquals(len(cloud_out.fields), 7, valid_points += 1
"PointCloud2 with channel INDEX: fields size != 7")
valid_points = 0 assert valid_points == cloud_out.width, \
for i in range(len(scan.ranges)): "Valid points != PointCloud2 width"
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, idx_x = idx_y = idx_z = 0
"Valid points != PointCloud2 width") idx_intensity = idx_index = 0
idx_distance = idx_stamps = 0
idx_x = idx_y = idx_z = 0 i = 0
idx_intensity = idx_index = 0 for f in cloud_out.fields:
idx_distance = idx_stamps = 0 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 i = 0
for f in cloud_out.fields: for point in pc2.read_points(cloud_out):
if f.name == "x": ri = scan.ranges[i]
idx_x = i ai = scan.angle_min + i * scan.angle_increment
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
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__': if __name__ == '__main__':
import rosunit pytest.main()
rosunit.unitrun(PKG, 'projection_test', ProjectionTest)