diff --git a/CMakeLists.txt b/CMakeLists.txt index 7cfcd033..6765664b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,16 +70,8 @@ install( ) if(BUILD_TESTING) - # TODO(Martin-Idel-SI): replace this with ament_lint_auto() and/or add the copyright linter back - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - ament_cppcheck() - ament_cpplint() - ament_lint_cmake() - ament_uncrustify() - + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(projection_test diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..309be1e1 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the 3-Clause BSD License, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/include/laser_geometry/laser_geometry.hpp b/include/laser_geometry/laser_geometry.hpp index f7ee51b7..fe91162a 100644 --- a/include/laser_geometry/laser_geometry.hpp +++ b/include/laser_geometry/laser_geometry.hpp @@ -1,32 +1,33 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * Copyright (c) 2018, Bosch Software Innovations GmbH. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2008, Willow Garage, Inc. +// Copyright (c) 2018, Bosch Software Innovations GmbH. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + #ifndef LASER_GEOMETRY__LASER_GEOMETRY_HPP_ #define LASER_GEOMETRY__LASER_GEOMETRY_HPP_ diff --git a/include/laser_geometry/visibility_control.hpp b/include/laser_geometry/visibility_control.hpp index 2b063ab1..10e1e7c7 100644 --- a/include/laser_geometry/visibility_control.hpp +++ b/include/laser_geometry/visibility_control.hpp @@ -1,36 +1,31 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2017, Open Source Robotics Foundation, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ +// Copyright (c) 2017, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #ifndef LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_ #define LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/package.xml b/package.xml index d0115865..79c8abf7 100644 --- a/package.xml +++ b/package.xml @@ -16,14 +16,14 @@ BSD + http://github.com/ros-perception/laser_geometry + Dave Hershberger Mabel Zhang Radu Bogdan Rusu Tully Foote William Woodall - http://ros.org/wiki/laser_geometry - ament_cmake eigen3_cmake_module @@ -43,12 +43,10 @@ sensor_msgs_py tf2 - ament_cmake_cppcheck - ament_cmake_cpplint + ament_lint_auto + ament_lint_common ament_cmake_gtest - ament_cmake_lint_cmake ament_cmake_pytest - ament_cmake_uncrustify python_cmake_module diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 1693a25c..4aff297b 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -1,32 +1,32 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * Copyright (c) 2018, Bosch Software Innovations GmbH. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2008, Willow Garage, Inc. +// Copyright (c) 2018, Bosch Software Innovations GmbH. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #include "laser_geometry/laser_geometry.hpp" diff --git a/src/laser_geometry/__init__.py b/src/laser_geometry/__init__.py index ff0168c5..500aa83b 100644 --- a/src/laser_geometry/__init__.py +++ b/src/laser_geometry/__init__.py @@ -1 +1,30 @@ -from .laser_geometry import LaserProjection +# Copyright (c) 2014, Enrique Fernandez +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from .laser_geometry import LaserProjection # noqa: F401 diff --git a/src/laser_geometry/laser_geometry.py b/src/laser_geometry/laser_geometry.py index 7c9cb071..ef2dcde4 100644 --- a/src/laser_geometry/laser_geometry.py +++ b/src/laser_geometry/laser_geometry.py @@ -1,42 +1,42 @@ -""" -Copyright (c) 2014, Enrique Fernandez -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the Willow Garage, Inc. nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. -""" +# Copyright (c) 2014, Enrique Fernandez +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +import numpy as np import rclpy import rclpy.logging from sensor_msgs.msg import PointCloud2 import sensor_msgs_py.point_cloud2 as pc2 -import numpy as np class LaserProjection: """ - A class to Project Laser Scan + A class to Project Laser Scan. This class will project laser scans into point clouds. It caches unit vectors between runs (provided the angular resolution of @@ -63,18 +63,18 @@ class LaserProjection: specific timestamp at which each point was measured. """ - LASER_SCAN_INVALID = -1.0 + LASER_SCAN_INVALID = -1.0 LASER_SCAN_MIN_RANGE = -2.0 LASER_SCAN_MAX_RANGE = -3.0 class ChannelOption: - NONE = 0x00 # Enable no channels - INTENSITY = 0x01 # Enable "intensities" channel - INDEX = 0x02 # Enable "index" channel - DISTANCE = 0x04 # Enable "distances" channel - TIMESTAMP = 0x08 # Enable "stamps" channel - VIEWPOINT = 0x10 # Enable "viewpoint" channel - DEFAULT = (INTENSITY | INDEX) + NONE = 0x00 # Enable no channels + INTENSITY = 0x01 # Enable "intensities" channel + INDEX = 0x02 # Enable "index" channel + DISTANCE = 0x04 # Enable "distances" channel + TIMESTAMP = 0x08 # Enable "stamps" channel + VIEWPOINT = 0x10 # Enable "viewpoint" channel + DEFAULT = (INTENSITY | INDEX) def __init__(self): self.__angle_min = 0.0 @@ -82,7 +82,8 @@ def __init__(self): self.__cos_sin_map = np.array([[]]) - def projectLaser(self, scan_in, + def projectLaser( + self, scan_in, range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT): """ Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2. @@ -91,7 +92,8 @@ def projectLaser(self, scan_in, point cloud. The generated cloud will be in the same frame as the original laser scan. - Keyword arguments: + Keyword Arguments. + scan_in -- The input laser scan. range_cutoff -- An additional range cutoff which can be applied which is more limiting than max_range in the scan @@ -106,14 +108,15 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): ranges = np.array(scan_in.ranges) if (self.__cos_sin_map.shape[1] != N or - self.__angle_min != scan_in.angle_min or - self.__angle_max != scan_in.angle_max): - rclpy.logging.get_logger("project_laser").debug( - "No precomputed map given. Computing one.") + self.__angle_min != scan_in.angle_min or + self.__angle_max != scan_in.angle_max): + + 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 - + angles = scan_in.angle_min + np.arange(N) * scan_in.angle_increment self.__cos_sin_map = np.array([np.cos(angles), np.sin(angles)]) @@ -124,31 +127,30 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): fields = [pc2.PointField() for _ in range(3)] - fields[0].name = "x" + fields[0].name = 'x' fields[0].offset = 0 fields[0].datatype = pc2.PointField.FLOAT32 fields[0].count = 1 - fields[1].name = "y" + fields[1].name = 'y' fields[1].offset = 4 fields[1].datatype = pc2.PointField.FLOAT32 fields[1].count = 1 - fields[2].name = "z" + fields[2].name = 'z' fields[2].offset = 8 fields[2].datatype = pc2.PointField.FLOAT32 fields[2].count = 1 - idx_intensity = idx_index = idx_distance = idx_timestamp = -1 + idx_intensity = idx_index = idx_distance = idx_timestamp = -1 idx_vpx = idx_vpy = idx_vpz = -1 offset = 12 - if (channel_options & self.ChannelOption.INTENSITY and - len(scan_in.intensities) > 0): + if (channel_options & self.ChannelOption.INTENSITY and len(scan_in.intensities) > 0): field_size = len(fields) fields.append(pc2.PointField()) - fields[field_size].name = "intensity" + fields[field_size].name = 'intensity' fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 @@ -158,7 +160,7 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): if channel_options & self.ChannelOption.INDEX: field_size = len(fields) fields.append(pc2.PointField()) - fields[field_size].name = "index" + fields[field_size].name = 'index' fields[field_size].datatype = pc2.PointField.INT32 fields[field_size].offset = offset fields[field_size].count = 1 @@ -168,7 +170,7 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): if channel_options & self.ChannelOption.DISTANCE: field_size = len(fields) fields.append(pc2.PointField()) - fields[field_size].name = "distances" + fields[field_size].name = 'distances' fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 @@ -178,7 +180,7 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): if channel_options & self.ChannelOption.TIMESTAMP: field_size = len(fields) fields.append(pc2.PointField()) - fields[field_size].name = "stamps" + fields[field_size].name = 'stamps' fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 @@ -188,7 +190,7 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): if channel_options & self.ChannelOption.VIEWPOINT: field_size = len(fields) fields.extend([pc2.PointField() for _ in range(3)]) - fields[field_size].name = "vp_x" + fields[field_size].name = 'vp_x' fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 @@ -196,7 +198,7 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): idx_vpx = field_size field_size += 1 - fields[field_size].name = "vp_y" + fields[field_size].name = 'vp_y' fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 @@ -204,7 +206,7 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): idx_vpy = field_size field_size += 1 - fields[field_size].name = "vp_z" + fields[field_size].name = 'vp_z' fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 @@ -243,4 +245,3 @@ def __projectLaser(self, scan_in, range_cutoff, channel_options): cloud_out = pc2.create_cloud(scan_in.header, fields, points) return cloud_out - diff --git a/test/projection_test.cpp b/test/projection_test.cpp index 4e1c9f1d..3b7b4728 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -1,32 +1,32 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * Copyright (c) 2018, Bosch Software Innovations GmbH. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2008, Willow Garage, Inc. +// Copyright (c) 2018, Bosch Software Innovations GmbH. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #define _USE_MATH_DEFINES #include diff --git a/test/projection_test.py b/test/projection_test.py index 21ac4e10..895e53d6 100755 --- a/test/projection_test.py +++ b/test/projection_test.py @@ -1,23 +1,51 @@ -#!/usr/bin/env python3 +# Copyright (c) 2014, Enrique Fernandez +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +from itertools import product + +from laser_geometry import LaserProjection +import numpy as np +import pytest 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 pytest +import sensor_msgs_py.point_cloud2 as pc2 -PROJECTION_TEST_RANGE_MIN = 0.23 +PROJECTION_TEST_RANGE_MIN = 0.23 PROJECTION_TEST_RANGE_MAX = 40.00 + class BuildScanException(Exception): pass + def build_constant_scan( range_val, intensity_val, angle_min, angle_max, angle_increment, scan_time): @@ -27,7 +55,7 @@ def build_constant_scan( scan = LaserScan() 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_max = angle_max scan.angle_increment = angle_increment @@ -40,8 +68,9 @@ def build_constant_scan( return scan + def test_project_laser(): - tolerance = 6 # decimal places + tolerance = 6 # decimal places projector = LaserProjection() ranges = [-1.0, 1.0, 5.0, 100.0] @@ -56,8 +85,10 @@ def test_project_laser(): 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): + product( + ranges, intensities, + min_angles, max_angles, + angle_increments, scan_times): try: scan = build_constant_scan( range_val, intensity_val, @@ -65,23 +96,21 @@ def test_project_laser(): except BuildScanException: assert (angle_max - angle_min)/angle_increment <= 0 - 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) + assert 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): + 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" + assert valid_points == cloud_out.width, 'Valid points != PointCloud2 width' idx_x = idx_y = idx_z = 0 idx_intensity = idx_index = 0 @@ -89,19 +118,19 @@ def test_project_laser(): i = 0 for f in cloud_out.fields: - if f.name == "x": + if f.name == 'x': idx_x = i - elif f.name == "y": + elif f.name == 'y': idx_y = i - elif f.name == "z": + elif f.name == 'z': idx_z = i - elif f.name == "intensity": + elif f.name == 'intensity': idx_intensity = i - elif f.name == "index": + elif f.name == 'index': idx_index = i - elif f.name == "distances": + elif f.name == 'distances': idx_distance = i - elif f.name == "stamps": + elif f.name == 'stamps': idx_stamps = i i += 1 @@ -110,20 +139,18 @@ def test_project_laser(): 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_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 * scan.time_increment, tolerance), 'Timestamp not equal' i += 1 + if __name__ == '__main__': pytest.main()