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

Added common linters (#96)

Browse Source 
Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
This commit is contained in:
Alejandro Hernández Cordero 2024-08-19 16:32:44 +02:00 committed by GitHub
parent ddf67cb25c
commit 6d79cd6546
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
10 changed files with 285 additions and 239 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
CMakeLists.txt
View File

@ -70,16 +70,8 @@ install(
) )
if(BUILD_TESTING) if(BUILD_TESTING)
# TODO(Martin-Idel-SI): replace this with ament_lint_auto() and/or add the copyright linter back find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_cppcheck REQUIRED) ament_lint_auto_find_test_dependencies()
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_cmake_gtest REQUIRED) find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(projection_test ament_add_gtest(projection_test

3
CONTRIBUTING.md Normal file
View File

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

59
include/laser_geometry/laser_geometry.hpp
View File

@ -1,32 +1,33 @@
/* // Copyright (c) 2008, Willow Garage, Inc.
* Copyright (c) 2008, Willow Garage, Inc. // Copyright (c) 2018, Bosch Software Innovations GmbH.
* Copyright (c) 2018, Bosch Software Innovations GmbH. // All rights reserved.
* All rights reserved. //
* // Redistribution and use in source and binary forms, with or without
* Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met:
* modification, are permitted provided that the following conditions are met: //
* // * Redistributions of source code must retain the above copyright
* * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer.
* notice, this list of conditions and the following disclaimer. //
* * Redistributions in binary form must reproduce the above copyright // * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the // notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution. // 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 // * Neither the name of the copyright holder nor the names of its
* this software without specific prior written permission. // 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 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* POSSIBILITY OF SUCH DAMAGE. // 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_ #ifndef LASER_GEOMETRY__LASER_GEOMETRY_HPP_
#define LASER_GEOMETRY__LASER_GEOMETRY_HPP_ #define LASER_GEOMETRY__LASER_GEOMETRY_HPP_

61
include/laser_geometry/visibility_control.hpp
View File

@ -1,36 +1,31 @@
/********************************************************************* // Copyright (c) 2017, Open Source Robotics Foundation, Inc.
* Software License Agreement (BSD License) // All rights reserved.
* //
* Copyright (c) 2017, Open Source Robotics Foundation, Inc. // Redistribution and use in source and binary forms, with or without
* All rights reserved. // modification, are permitted provided that the following conditions are met:
* //
* Redistribution and use in source and binary forms, with or without // * Redistributions of source code must retain the above copyright
* modification, are permitted provided that the following conditions // notice, this list of conditions and the following disclaimer.
* are met: //
* // * Redistributions in binary form must reproduce the above copyright
* * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer in the
* notice, this list of conditions and the following disclaimer. // documentation and/or other materials provided with the distribution.
* * Redistributions in binary form must reproduce the above //
* copyright notice, this list of conditions and the following // * Neither the name of the copyright holder nor the names of its
* disclaimer in the documentation and/or other materials provided // contributors may be used to endorse or promote products derived from
* with the distribution. // this software without specific prior written permission.
* * Neither the name of the copyright holder nor the names of its //
* contributors may be used to endorse or promote products derived // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* from this software without specific prior written permission. // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER // POSSIBILITY OF SUCH DAMAGE.
* 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_ #ifndef LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_
#define LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_ #define LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_

10
package.xml
View File

@ -16,14 +16,14 @@
<license>BSD</license> <license>BSD</license>
<url type="website">http://github.com/ros-perception/laser_geometry</url>
<author email="dave.hershberger@sri.com">Dave Hershberger</author> <author email="dave.hershberger@sri.com">Dave Hershberger</author>
<author email="mabel@openrobotics.org">Mabel Zhang</author> <author email="mabel@openrobotics.org">Mabel Zhang</author>
<author>Radu Bogdan Rusu</author> <author>Radu Bogdan Rusu</author>
<author>Tully Foote</author> <author>Tully Foote</author>
<author email="william@osrfoundation.org">William Woodall</author> <author email="william@osrfoundation.org">William Woodall</author>
<url>http://ros.org/wiki/laser_geometry</url>
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend> <buildtool_depend>eigen3_cmake_module</buildtool_depend>
@ -43,12 +43,10 @@
<exec_depend>sensor_msgs_py</exec_depend> <exec_depend>sensor_msgs_py</exec_depend>
<exec_depend>tf2</exec_depend> <exec_depend>tf2</exec_depend>
<test_depend>ament_cmake_cppcheck</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_cmake_cpplint</test_depend> <test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_gtest</test_depend> <test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_lint_cmake</test_depend>
<test_depend>ament_cmake_pytest</test_depend> <test_depend>ament_cmake_pytest</test_depend>
<test_depend>ament_cmake_uncrustify</test_depend>
<test_depend>python_cmake_module</test_depend> <test_depend>python_cmake_module</test_depend>
<export> <export>

58
src/laser_geometry.cpp
View File

@ -1,32 +1,32 @@
/* // Copyright (c) 2008, Willow Garage, Inc.
* Copyright (c) 2008, Willow Garage, Inc. // Copyright (c) 2018, Bosch Software Innovations GmbH.
* Copyright (c) 2018, Bosch Software Innovations GmbH. // All rights reserved.
* All rights reserved. //
* // Redistribution and use in source and binary forms, with or without
* Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met:
* modification, are permitted provided that the following conditions are met: //
* // * Redistributions of source code must retain the above copyright
* * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer.
* notice, this list of conditions and the following disclaimer. //
* * Redistributions in binary form must reproduce the above copyright // * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the // notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution. // 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 // * Neither the name of the copyright holder nor the names of its
* this software without specific prior written permission. // 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 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* POSSIBILITY OF SUCH DAMAGE. // 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" #include "laser_geometry/laser_geometry.hpp"

31
src/laser_geometry/__init__.py
View File

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

95
src/laser_geometry/laser_geometry.py
View File

@ -1,42 +1,42 @@
""" # Copyright (c) 2014, Enrique Fernandez
Copyright (c) 2014, Enrique Fernandez # All rights reserved.
All rights reserved. #
# Redistribution and use in source and binary forms, with or without
Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met:
modification, are permitted provided that the following conditions are met: #
# * Redistributions of source code must retain the above copyright
* Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer.
notice, this list of conditions and the following disclaimer. #
* Redistributions in binary form must reproduce the above copyright # * Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the # notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution. # 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 # * Neither the name of the copyright holder nor the names of its
this software without specific prior written permission. # 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 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
POSSIBILITY OF SUCH DAMAGE. # 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
import rclpy.logging import rclpy.logging
from sensor_msgs.msg import PointCloud2 from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2 import sensor_msgs_py.point_cloud2 as pc2
import numpy as np
class LaserProjection: 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 This class will project laser scans into point clouds. It caches
unit vectors between runs (provided the angular resolution of unit vectors between runs (provided the angular resolution of
@ -82,7 +82,8 @@ class LaserProjection:
self.__cos_sin_map = np.array([[]]) self.__cos_sin_map = np.array([[]])
def projectLaser(self, scan_in, def projectLaser(
self, scan_in,
range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT): range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT):
""" """
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2. Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2.
@ -91,7 +92,8 @@ class LaserProjection:
point cloud. The generated cloud will be in the same frame point cloud. The generated cloud will be in the same frame
as the original laser scan. as the original laser scan.
Keyword arguments: Keyword Arguments.
scan_in -- The input laser scan. scan_in -- The input laser scan.
range_cutoff -- An additional range cutoff which can be range_cutoff -- An additional range cutoff which can be
applied which is more limiting than max_range in the scan applied which is more limiting than max_range in the scan
@ -108,8 +110,9 @@ 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):
rclpy.logging.get_logger("project_laser").debug(
"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
@ -124,17 +127,17 @@ class LaserProjection:
fields = [pc2.PointField() for _ in range(3)] fields = [pc2.PointField() for _ in range(3)]
fields[0].name = "x" fields[0].name = 'x'
fields[0].offset = 0 fields[0].offset = 0
fields[0].datatype = pc2.PointField.FLOAT32 fields[0].datatype = pc2.PointField.FLOAT32
fields[0].count = 1 fields[0].count = 1
fields[1].name = "y" fields[1].name = 'y'
fields[1].offset = 4 fields[1].offset = 4
fields[1].datatype = pc2.PointField.FLOAT32 fields[1].datatype = pc2.PointField.FLOAT32
fields[1].count = 1 fields[1].count = 1
fields[2].name = "z" fields[2].name = 'z'
fields[2].offset = 8 fields[2].offset = 8
fields[2].datatype = pc2.PointField.FLOAT32 fields[2].datatype = pc2.PointField.FLOAT32
fields[2].count = 1 fields[2].count = 1
@ -144,11 +147,10 @@ class LaserProjection:
offset = 12 offset = 12
if (channel_options & self.ChannelOption.INTENSITY and if (channel_options & self.ChannelOption.INTENSITY and len(scan_in.intensities) > 0):
len(scan_in.intensities) > 0):
field_size = len(fields) field_size = len(fields)
fields.append(pc2.PointField()) fields.append(pc2.PointField())
fields[field_size].name = "intensity" fields[field_size].name = 'intensity'
fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].datatype = pc2.PointField.FLOAT32
fields[field_size].offset = offset fields[field_size].offset = offset
fields[field_size].count = 1 fields[field_size].count = 1
@ -158,7 +160,7 @@ class LaserProjection:
if channel_options & self.ChannelOption.INDEX: if channel_options & self.ChannelOption.INDEX:
field_size = len(fields) field_size = len(fields)
fields.append(pc2.PointField()) fields.append(pc2.PointField())
fields[field_size].name = "index" fields[field_size].name = 'index'
fields[field_size].datatype = pc2.PointField.INT32 fields[field_size].datatype = pc2.PointField.INT32
fields[field_size].offset = offset fields[field_size].offset = offset
fields[field_size].count = 1 fields[field_size].count = 1
@ -168,7 +170,7 @@ class LaserProjection:
if channel_options & self.ChannelOption.DISTANCE: if channel_options & self.ChannelOption.DISTANCE:
field_size = len(fields) field_size = len(fields)
fields.append(pc2.PointField()) fields.append(pc2.PointField())
fields[field_size].name = "distances" fields[field_size].name = 'distances'
fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].datatype = pc2.PointField.FLOAT32
fields[field_size].offset = offset fields[field_size].offset = offset
fields[field_size].count = 1 fields[field_size].count = 1
@ -178,7 +180,7 @@ class LaserProjection:
if channel_options & self.ChannelOption.TIMESTAMP: if channel_options & self.ChannelOption.TIMESTAMP:
field_size = len(fields) field_size = len(fields)
fields.append(pc2.PointField()) fields.append(pc2.PointField())
fields[field_size].name = "stamps" fields[field_size].name = 'stamps'
fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].datatype = pc2.PointField.FLOAT32
fields[field_size].offset = offset fields[field_size].offset = offset
fields[field_size].count = 1 fields[field_size].count = 1
@ -188,7 +190,7 @@ class LaserProjection:
if channel_options & self.ChannelOption.VIEWPOINT: if channel_options & self.ChannelOption.VIEWPOINT:
field_size = len(fields) field_size = len(fields)
fields.extend([pc2.PointField() for _ in range(3)]) 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].datatype = pc2.PointField.FLOAT32
fields[field_size].offset = offset fields[field_size].offset = offset
fields[field_size].count = 1 fields[field_size].count = 1
@ -196,7 +198,7 @@ class LaserProjection:
idx_vpx = field_size idx_vpx = field_size
field_size += 1 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].datatype = pc2.PointField.FLOAT32
fields[field_size].offset = offset fields[field_size].offset = offset
fields[field_size].count = 1 fields[field_size].count = 1
@ -204,7 +206,7 @@ class LaserProjection:
idx_vpy = field_size idx_vpy = field_size
field_size += 1 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].datatype = pc2.PointField.FLOAT32
fields[field_size].offset = offset fields[field_size].offset = offset
fields[field_size].count = 1 fields[field_size].count = 1
@ -243,4 +245,3 @@ class LaserProjection:
cloud_out = pc2.create_cloud(scan_in.header, fields, points) cloud_out = pc2.create_cloud(scan_in.header, fields, points)
return cloud_out return cloud_out

58
test/projection_test.cpp
View File

@ -1,32 +1,32 @@
/* // Copyright (c) 2008, Willow Garage, Inc.
* Copyright (c) 2008, Willow Garage, Inc. // Copyright (c) 2018, Bosch Software Innovations GmbH.
* Copyright (c) 2018, Bosch Software Innovations GmbH. // All rights reserved.
* All rights reserved. //
* // Redistribution and use in source and binary forms, with or without
* Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met:
* modification, are permitted provided that the following conditions are met: //
* // * Redistributions of source code must retain the above copyright
* * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer.
* notice, this list of conditions and the following disclaimer. //
* * Redistributions in binary form must reproduce the above copyright // * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the // notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution. // 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 // * Neither the name of the copyright holder nor the names of its
* this software without specific prior written permission. // 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 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* POSSIBILITY OF SUCH DAMAGE. // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*/ // POSSIBILITY OF SUCH DAMAGE.
#define _USE_MATH_DEFINES #define _USE_MATH_DEFINES
#include <gtest/gtest.h> #include <gtest/gtest.h>

101
test/projection_test.py
View File

@ -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
import rclpy.duration import rclpy.duration
import rclpy.time import rclpy.time
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 import sensor_msgs_py.point_cloud2 as pc2
import numpy as np
from itertools import product
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(Exception): class BuildScanException(Exception):
pass pass
def build_constant_scan( def build_constant_scan(
range_val, intensity_val, range_val, intensity_val,
angle_min, angle_max, angle_increment, scan_time): angle_min, angle_max, angle_increment, scan_time):
@ -27,7 +55,7 @@ def build_constant_scan(
scan = LaserScan() scan = LaserScan()
scan.header.stamp = rclpy.time.Time(seconds=10.10).to_msg() 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
@ -40,6 +68,7 @@ def build_constant_scan(
return scan return scan
def test_project_laser(): def test_project_laser():
tolerance = 6 # decimal places tolerance = 6 # decimal places
projector = LaserProjection() projector = LaserProjection()
@ -56,8 +85,10 @@ def test_project_laser():
for range_val, intensity_val, \ for range_val, intensity_val, \
angle_min, angle_max, angle_increment, scan_time in \ angle_min, angle_max, angle_increment, scan_time in \
product(ranges, intensities, product(
min_angles, max_angles, angle_increments, scan_times): ranges, intensities,
min_angles, max_angles,
angle_increments, scan_times):
try: try:
scan = build_constant_scan( scan = build_constant_scan(
range_val, intensity_val, range_val, intensity_val,
@ -65,23 +96,21 @@ def test_project_laser():
except BuildScanException: except BuildScanException:
assert (angle_max - angle_min)/angle_increment <= 0 assert (angle_max - angle_min)/angle_increment <= 0
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 |
LaserProjection.ChannelOption.TIMESTAMP) LaserProjection.ChannelOption.TIMESTAMP)
assert len(cloud_out.fields) == 7, \ assert len(cloud_out.fields) == 7, 'PointCloud2 with channel INDEX: fields size != 7'
"PointCloud2 with channel INDEX: fields size != 7"
valid_points = 0 valid_points = 0
for i in range(len(scan.ranges)): for i in range(len(scan.ranges)):
ri = scan.ranges[i] ri = scan.ranges[i]
if (PROJECTION_TEST_RANGE_MIN <= ri and if (PROJECTION_TEST_RANGE_MIN <= ri and ri <= PROJECTION_TEST_RANGE_MAX):
ri <= PROJECTION_TEST_RANGE_MAX):
valid_points += 1 valid_points += 1
assert valid_points == cloud_out.width, \ assert valid_points == cloud_out.width, 'Valid points != PointCloud2 width'
"Valid points != PointCloud2 width"
idx_x = idx_y = idx_z = 0 idx_x = idx_y = idx_z = 0
idx_intensity = idx_index = 0 idx_intensity = idx_index = 0
@ -89,19 +118,19 @@ def test_project_laser():
i = 0 i = 0
for f in cloud_out.fields: for f in cloud_out.fields:
if f.name == "x": if f.name == 'x':
idx_x = i idx_x = i
elif f.name == "y": elif f.name == 'y':
idx_y = i idx_y = i
elif f.name == "z": elif f.name == 'z':
idx_z = i idx_z = i
elif f.name == "intensity": elif f.name == 'intensity':
idx_intensity = i idx_intensity = i
elif f.name == "index": elif f.name == 'index':
idx_index = i idx_index = i
elif f.name == "distances": elif f.name == 'distances':
idx_distance = i idx_distance = i
elif f.name == "stamps": elif f.name == 'stamps':
idx_stamps = i idx_stamps = i
i += 1 i += 1
@ -110,20 +139,18 @@ def test_project_laser():
ri = scan.ranges[i] ri = scan.ranges[i]
ai = scan.angle_min + i * scan.angle_increment ai = scan.angle_min + i * scan.angle_increment
assert point[idx_x] == pytest.approx(ri * np.cos(ai), assert point[idx_x] == pytest.approx(ri * np.cos(ai), abs=tolerance), 'x not equal'
abs=tolerance), "x not equal" assert point[idx_y] == pytest.approx(ri * np.sin(ai), tolerance), 'y not equal'
assert point[idx_y] == pytest.approx(ri * np.sin(ai), assert point[idx_z] == pytest.approx(0, tolerance), 'z not equal'
tolerance), "y not equal" assert point[idx_intensity] == pytest.approx(
assert point[idx_z] == pytest.approx(0, tolerance), "z not equal" scan.intensities[i],
assert point[idx_intensity] == pytest.approx(scan.intensities[i], tolerance), 'Intensity not equal'
tolerance), "Intensity not equal" assert point[idx_index] == pytest.approx(i, tolerance), 'Index not equal'
assert point[idx_index] == pytest.approx(i, tolerance), \ assert point[idx_distance] == pytest.approx(ri, tolerance), 'Distance not equal'
"Index not equal"
assert point[idx_distance] == pytest.approx(ri, tolerance), \
"Distance not equal"
assert point[idx_stamps] == pytest.approx( assert point[idx_stamps] == pytest.approx(
i * scan.time_increment, tolerance), "Timestamp not equal" i * scan.time_increment, tolerance), 'Timestamp not equal'
i += 1 i += 1
if __name__ == '__main__': if __name__ == '__main__':
pytest.main() pytest.main()