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

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

61
include/laser_geometry/visibility_control.hpp
View File

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

10
package.xml
View File

@ -16,14 +16,14 @@
<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="mabel@openrobotics.org">Mabel Zhang</author>
<author>Radu Bogdan Rusu</author>
<author>Tully Foote</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>eigen3_cmake_module</buildtool_depend>
@ -43,12 +43,10 @@
<exec_depend>sensor_msgs_py</exec_depend>
<exec_depend>tf2</exec_depend>
<test_depend>ament_cmake_cppcheck</test_depend>
<test_depend>ament_cmake_cpplint</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</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_uncrustify</test_depend>
<test_depend>python_cmake_module</test_depend>
<export>

58
src/laser_geometry.cpp
View File

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

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

119
src/laser_geometry/laser_geometry.py
View File

@ -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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
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 @@ class LaserProjection:
cloud_out = pc2.create_cloud(scan_in.header, fields, points)
return cloud_out

58
test/projection_test.cpp
View File

@ -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 <gtest/gtest.h>

113
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.duration
import rclpy.time
import sensor_msgs_py.point_cloud2 as pc2
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
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()