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

3
package.xml
View File

@ -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
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.
"""
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

91
test/projection_test.py
View File

@ -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,23 +26,21 @@ 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(self):
def test_project_laser():
tolerance = 6 # decimal places
projector = LaserProjection()
@ -54,7 +52,7 @@ class ProjectionTest(unittest.TestCase):
angle_increments = np.pi / np.array([180., 360., 720.])
scan_times = [rospy.Duration(1./i) for i in [40, 20]]
scan_times = [rclpy.duration.Duration(seconds=1./i) for i in [40, 20]]
for range_val, intensity_val, \
angle_min, angle_max, angle_increment, scan_time in \
@ -65,42 +63,41 @@ class ProjectionTest(unittest.TestCase):
range_val, intensity_val,
angle_min, angle_max, angle_increment, scan_time)
except BuildScanException:
if (angle_max - angle_min)/angle_increment > 0:
self.fail()
assert (angle_max - angle_min)/angle_increment <= 0
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INDEX)
self.assertEquals(len(cloud_out.fields), 4,
"PointCloud2 with channel INDEX: fields size != 4")
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")
assert 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")
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)
self.assertEquals(len(cloud_out.fields), 5,
"PointCloud2 with channel INDEX: fields size != 5")
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 |
LaserProjection.ChannelOption.DISTANCE)
self.assertEquals(len(cloud_out.fields), 6,
"PointCloud2 with channel INDEX: fields size != 6")
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 |
LaserProjection.ChannelOption.TIMESTAMP)
self.assertEquals(len(cloud_out.fields), 7,
"PointCloud2 with channel INDEX: fields size != 7")
assert len(cloud_out.fields) == 7, \
"PointCloud2 with channel INDEX: fields size != 7"
valid_points = 0
for i in range(len(scan.ranges)):
@ -109,8 +106,8 @@ class ProjectionTest(unittest.TestCase):
ri <= PROJECTION_TEST_RANGE_MAX):
valid_points += 1
self.assertEqual(valid_points, cloud_out.width,
"Valid points != PointCloud2 width")
assert valid_points == cloud_out.width, \
"Valid points != PointCloud2 width"
idx_x = idx_y = idx_z = 0
idx_intensity = idx_index = 0
@ -139,25 +136,21 @@ class ProjectionTest(unittest.TestCase):
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")
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()