diff --git a/.github/workflows/mirror-rolling-to-ros2.yaml b/.github/workflows/mirror-rolling-to-ros2.yaml new file mode 100644 index 00000000..fa4e69bf --- /dev/null +++ b/.github/workflows/mirror-rolling-to-ros2.yaml @@ -0,0 +1,13 @@ +name: Mirror rolling to ros2 + +on: + push: + branches: [ rolling ] + +jobs: + mirror-to-ros2: + runs-on: ubuntu-latest + steps: + - uses: zofrex/mirror-branch@v1 + with: + target-branch: ros2 diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 755dbb14..78036859 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,77 @@ Changelog for package laser_geometry ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.1 (2024-10-03) +------------------ +* Added common linters (`#96 `_) +* Contributors: Alejandro Hernández Cordero + +2.8.0 (2024-04-26) +------------------ + +2.7.0 (2023-12-26) +------------------ +* Switch to target_link_libraries. (`#92 `_) +* Contributors: Chris Lalancette + +2.6.0 (2023-04-28) +------------------ + +2.5.0 (2023-02-14) +------------------ +* Update laser_geometry to C++17. (`#90 `_) +* Update Maintainers (`#88 `_) +* Mirror rolling to ros2 +* Contributors: Audrow Nash, Chris Lalancette + +2.4.0 (2022-03-01) +------------------ +* Install headers to include/${PROJECT_NAME} (`#86 `_) +* Explicit cast to double to prevent loss of precision +* Fix Duration casting issue leading to no undistortion +* Contributors: Jonathan Binney, Marco Lampacrescia, Shane Loretz + +2.3.0 (2022-01-14) +------------------ +* Fix building on running on Windows Debug (`#82 `_) +* Update python code and tests for ros2 (`#80 `_) +* Contributors: Chris Lalancette, Jonathan Binney + +2.2.2 (2021-05-11) +------------------ +* Export sensor_msgs, tf2, and rclcpp as dependencies +* Contributors: Mabel Zhang, Michel Hidalgo + +2.2.1 (2020-12-08) +------------------ +* Use rclcpp::Duration::from_seconds (`#72 `_) +* update maintainers +* increase test timeout +* Contributors: Dirk Thomas, Ivan Santiago Paunovic, Jonathan Binney, Mabel Zhang + +2.2.0 (2020-04-30) +------------------ +* use ament_export_targets() +* code style only: wrap after open parenthesis if not in one line (`#52 `_) +* use target_include_directories +* Drop CMake extras redundant with eigen3_cmake_module. (`#50 `_) +* Contributors: Dirk Thomas, Jonathan Binney, Karsten Knese, Michel Hidalgo + +2.1.0 (2019-09-27) +------------------ +* Merge pull request `#46 `_ from sloretz/eigen3_cmake_module +* Contributors: Jonathan Binney, Shane Loretz + +2.0.0 (2018-06-27) +------------------ +* Removed the ``angle`` dependency as no longer necessary. +* Updated to build statically but use position independent code. +* Updated to compile, and to remove PointCloud support, and remove boost. +* Added visibility headers modified from ``rclcpp``. +* Updated ``laser_geometry`` to build for ros2 (and on Windows 10). +* Improved use of numpy. (`#14 `_) +* Contributors: Alessandro Bottero, Andreas Greimel, Brian Fjeldstad, Eric Wieser, Jon Binney, Jonathan Binney, Martin Idel, Mikael Arguedas, Vincent Rabaud, William Woodall + 1.6.4 (2015-05-18) ------------------ * Fix segfault when laserscan ranges[] is empty diff --git a/CMakeLists.txt b/CMakeLists.txt index a502c73d..f245c3bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,50 +2,91 @@ cmake_minimum_required(VERSION 3.5) project(laser_geometry) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) -find_package(angles REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(tf2 REQUIRED) -find_package(ecl_eigen REQUIRED) - -# TODO(dhood): enable python support once ported to ROS 2 -# catkin_python_setup() - -# -#catkin_package( -# INCLUDE_DIRS include -# LIBRARIES laser_geometry -# DEPENDS Boost Eigen -#) -# - -include_directories(include - ${angles_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${sensor_msgs_INCLUDE_DIRS} - ${tf2_INCLUDE_DIRS} - ${tf2_eigen_INCLUDE_DIRS} - $ENV{BOOST_ROOT} +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_python_install_package(laser_geometry PACKAGE_DIR src/laser_geometry) + +add_library(laser_geometry SHARED src/laser_geometry.cpp) +target_include_directories(laser_geometry + PUBLIC + $ + $ + ${Eigen3_INCLUDE_DIRS} ) +target_link_libraries(laser_geometry PUBLIC + ${sensor_msgs_TARGETS} + tf2::tf2 +) +if(TARGET Eigen3::Eigen) + target_link_libraries(laser_geometry PUBLIC Eigen3::Eigen) +else() + target_include_directories(laser_geometry PUBLIC ${Eigen3_INCLUDE_DIRS}) +endif() -add_library(laser_geometry src/laser_geometry.cpp) -target_link_libraries(laser_geometry - ${angles_LIBRARIES} - ${rclcpp_LIBRARIES} - ${sensor_msgs_LIBRARIES} - ${tf2_LIBRARIES} - ${tf2_eigen_LIBRARIES} +target_link_libraries(laser_geometry PRIVATE + rclcpp::rclcpp ) -ament_export_include_directories(include) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(laser_geometry PRIVATE "LASER_GEOMETRY_BUILDING_LIBRARY") + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(laser_geometry) -ament_package() -install(TARGETS laser_geometry +# Export modern CMake targets +ament_export_targets(laser_geometry) + +ament_export_dependencies( + Eigen3 + sensor_msgs + tf2 +) + +install( + TARGETS laser_geometry + EXPORT laser_geometry ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib) -install(DIRECTORY include/laser_geometry/ - DESTINATION include/${PROJECT_NAME}/ - FILES_MATCHING PATTERN "*.h") + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(projection_test + test/projection_test.cpp + TIMEOUT 240) + if(TARGET projection_test) + target_link_libraries(projection_test laser_geometry rclcpp::rclcpp) + endif() + + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(projection test/projection_test.py + TIMEOUT 120 + ) +endif() + +ament_package() diff --git a/CODEOWNERS b/CODEOWNERS new file mode 100644 index 00000000..d490d00d --- /dev/null +++ b/CODEOWNERS @@ -0,0 +1,2 @@ +# This file was generated by https://github.com/audrow/update-ros2-repos +* @quarkytale 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/LICENSE b/LICENSE new file mode 100644 index 00000000..d79557ee --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2008, Willow Garage, 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. diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h deleted file mode 100644 index d5623b5c..00000000 --- a/include/laser_geometry/laser_geometry.h +++ /dev/null @@ -1,317 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, 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 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. - */ - -#ifndef LASER_SCAN_UTILS_LASERSCAN_H -#define LASER_SCAN_UTILS_LASERSCAN_H - -#include -#include -#include - -#include "boost/numeric/ublas/matrix.hpp" -#include "boost/thread/mutex.hpp" - -#include -#include "sensor_msgs/msg/Laser_Scan.hpp" -#include "sensor_msgs/msg/Point_Cloud.hpp" -#include - -#ifndef ROS_DEBUG -#define ROS_DEBUG(...) -#endif // !ROS_DEBUG -#ifndef ROS_ASSERT -#define ROS_ASSERT(...) -#endif // !ROS_ASSERT - -#include - -namespace laser_geometry -{ - // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle) - const float LASER_SCAN_INVALID = -1.0; - const float LASER_SCAN_MIN_RANGE = -2.0; - const float LASER_SCAN_MAX_RANGE = -3.0; - - namespace channel_option - { - //! Enumerated output channels options. - /*! - * An OR'd set of these options is passed as the final argument of - * the projectLaser and transformLaserScanToPointCloud calls to - * enable generation of the appropriate set of additional channels. - */ - enum 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) //!< Enable "intensities" and "index" channels - }; - } - - //! \brief 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 - * your scanner is not changing) to avoid excess computation. - * - * By default all range values less than the scanner min_range, and - * greater than the scanner max_range are removed from the generated - * point cloud, as these are assumed to be invalid. - * - * If it is important to preserve a mapping between the index of - * range values and points in the cloud, the recommended approach is - * to pre-filter your laser_scan message to meet the requiremnt that all - * ranges are between min and max_range. - * - * The generated PointClouds have a number of channels which can be enabled - * through the use of ChannelOption. - * - channel_option::Intensity - Create a channel named "intensities" with the intensity of the return for each point - * - channel_option::Index - Create a channel named "index" containing the index from the original array for each point - * - channel_option::Distance - Create a channel named "distances" containing the distance from the laser to each point - * - channel_option::Timestamp - Create a channel named "stamps" containing the specific timestamp at which each point was measured - */ - class LaserProjection - { - - public: - - LaserProjection() : angle_min_(0), angle_max_(0) {} - - //! Destructor to deallocate stored unit vectors - ~LaserProjection(); - - //! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud - /*! - * Project a single laser scan from a linear array into a 3D - * point cloud. The generated cloud will be in the same frame - * as the original laser scan. - * - * \param scan_in The input laser scan - * \param cloud_out The output point cloud - * \param range_cutoff An additional range cutoff which can be - * applied to discard everything above it. - * Defaults to -1.0, which means the laser scan max range. - * \param channel_option An OR'd set of channels to include. - * Options include: channel_option::Default, - * channel_option::Intensity, channel_option::Index, - * channel_option::Distance, channel_option::Timestamp. - */ - void projectLaser (const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud& cloud_out, - double range_cutoff = -1.0, - int channel_options = channel_option::Default) - { - return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options); - } - - //! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 - /*! - * Project a single laser scan from a linear array into a 3D - * point cloud. The generated cloud will be in the same frame - * as the original laser scan. - * - * \param scan_in The input laser scan - * \param cloud_out The output point cloud - * \param range_cutoff An additional range cutoff which can be - * applied to discard everything above it. - * Defaults to -1.0, which means the laser scan max range. - * \param channel_option An OR'd set of channels to include. - * Options include: channel_option::Default, - * channel_option::Intensity, channel_option::Index, - * channel_option::Distance, channel_option::Timestamp. - */ - void projectLaser (const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - double range_cutoff = -1.0, - int channel_options = channel_option::Default) - { - projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); - } - - //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud in target frame - /*! - * Transform a single laser scan from a linear array into a 3D - * point cloud, accounting for movement of the laser over the - * course of the scan. In order for this transform to be - * meaningful at a single point in time, the target_frame must - * be a fixed reference frame. See the tf documentation for - * more information on fixed frames. - * - * \param target_frame The frame of the resulting point cloud - * \param scan_in The input laser scan - * \param cloud_out The output point cloud - * \param tf a tf::Transformer object to use to perform the - * transform - * \param range_cutoff An additional range cutoff which can be - * applied to discard everything above it. - * \param channel_option An OR'd set of channels to include. - * Options include: channel_option::Default, - * channel_option::Intensity, channel_option::Index, - * channel_option::Distance, channel_option::Timestamp. - */ - void transformLaserScanToPointCloud (const std::string& target_frame, - const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud& cloud_out, - tf2::BufferCore &tf, - double range_cutoff, - int channel_options = channel_option::Default) - { - return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options); - } - - //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud in target frame - /*! - * Transform a single laser scan from a linear array into a 3D - * point cloud, accounting for movement of the laser over the - * course of the scan. In order for this transform to be - * meaningful at a single point in time, the target_frame must - * be a fixed reference frame. See the tf documentation for - * more information on fixed frames. - * - * \param target_frame The frame of the resulting point cloud - * \param scan_in The input laser scan - * \param cloud_out The output point cloud - * \param tf a tf::Transformer object to use to perform the - * transform - * \param channel_option An OR'd set of channels to include. - * Options include: channel_option::Default, - * channel_option::Intensity, channel_option::Index, - * channel_option::Distance, channel_option::Timestamp. - */ - void transformLaserScanToPointCloud (const std::string& target_frame, - const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud& cloud_out, - tf2::BufferCore &tf, - int channel_options = channel_option::Default) - { - return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options); - } - - //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 in target frame - /*! - * Transform a single laser scan from a linear array into a 3D - * point cloud, accounting for movement of the laser over the - * course of the scan. In order for this transform to be - * meaningful at a single point in time, the target_frame must - * be a fixed reference frame. See the tf documentation for - * more information on fixed frames. - * - * \param target_frame The frame of the resulting point cloud - * \param scan_in The input laser scan - * \param cloud_out The output point cloud - * \param tf a tf2::BufferCore object to use to perform the - * transform - * \param range_cutoff An additional range cutoff which can be - * applied to discard everything above it. - * Defaults to -1.0, which means the laser scan max range. - * \param channel_option An OR'd set of channels to include. - * Options include: channel_option::Default, - * channel_option::Intensity, channel_option::Index, - * channel_option::Distance, channel_option::Timestamp. - */ - void transformLaserScanToPointCloud(const std::string &target_frame, - const sensor_msgs::msg::LaserScan &scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - tf2::BufferCore &tf, - double range_cutoff = -1.0, - int channel_options = channel_option::Default) - { - transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); - } - - protected: - - //! Internal protected representation of getUnitVectors - /*! - * This function should not be used by external users, however, - * it is left protected so that test code can evaluate it - * appropriately. - */ - const boost::numeric::ublas::matrix& getUnitVectors_(double angle_min, - double angle_max, - double angle_increment, - unsigned int length); - - private: - - //! Internal hidden representation of projectLaser - void projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud& cloud_out, - double range_cutoff, - bool preservative, - int channel_options); - - //! Internal hidden representation of projectLaser - void projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - double range_cutoff, - int channel_options); - - //! Internal hidden representation of transformLaserScanToPointCloud - void transformLaserScanToPointCloud_ (const std::string& target_frame, - sensor_msgs::msg::PointCloud& cloud_out, - const sensor_msgs::msg::LaserScan& scan_in, - tf2::BufferCore &tf, - double range_cutoff, - int channel_options); - - //! Internal hidden representation of transformLaserScanToPointCloud2 - void transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::msg::LaserScan &scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - tf2::BufferCore &tf, - double range_cutoff, - int channel_options); - - //! Function used by the several forms of transformLaserScanToPointCloud_ - void transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::msg::LaserScan &scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - tf2::Quaternion quat_start, - tf2::Vector3 origin_start, - tf2::Quaternion quat_end, - tf2::Vector3 origin_end, - double range_cutoff, - int channel_options); - - //! Internal map of pointers to stored values - std::map* > unit_vector_map_; - float angle_min_; - float angle_max_; - Eigen::ArrayXXd co_sine_map_; - boost::mutex guv_mutex_; - }; - -} - -#endif //LASER_SCAN_UTILS_LASERSCAN_H diff --git a/include/laser_geometry/laser_geometry.hpp b/include/laser_geometry/laser_geometry.hpp new file mode 100644 index 00000000..fe91162a --- /dev/null +++ b/include/laser_geometry/laser_geometry.hpp @@ -0,0 +1,204 @@ +// 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_ + +#include +#include +#include +#include + +#include // NOLINT (cpplint cannot handle include order here) + +#include "tf2/buffer_core.h" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "laser_geometry/visibility_control.hpp" + +namespace laser_geometry +{ +// NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle) +const float LASER_SCAN_INVALID = -1.0; +const float LASER_SCAN_MIN_RANGE = -2.0; +const float LASER_SCAN_MAX_RANGE = -3.0; + +namespace channel_option +{ +// Enumerated output channels options. +/*! + * An OR'd set of these options is passed as the final argument of + * the projectLaser and transformLaserScanToPointCloud calls to + * enable generation of the appropriate set of additional channels. + */ +enum 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) //!< Enable "intensities" and "index" channels +}; +} // namespace channel_option + +//! \brief 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 + * your scanner is not changing) to avoid excess computation. + * + * By default all range values less than the scanner min_range, and + * greater than the scanner max_range are removed from the generated + * point cloud, as these are assumed to be invalid. + * + * If it is important to preserve a mapping between the index of + * range values and points in the cloud, the recommended approach is + * to pre-filter your laser_scan message to meet the requiremnt that all + * ranges are between min and max_range. + * + * The generated PointClouds have a number of channels which can be enabled + * through the use of ChannelOption. + * - channel_option::Intensity - Create a channel named "intensities" with the intensity of the return for each point + * - channel_option::Index - Create a channel named "index" containing the index from the original array for each point + * - channel_option::Distance - Create a channel named "distances" containing the distance from the laser to each point + * - channel_option::Timestamp - Create a channel named "stamps" containing the specific timestamp at which each point was measured + */ + +// TODO(Martin-Idel-SI): the support for PointCloud1 has been removed for now. +// Refer to the GitHub issue #29: https://github.com/ros-perception/laser_geometry/issues/29 + +class LaserProjection +{ +public: + LaserProjection() + : angle_min_(0), angle_max_(0) {} + + // Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 + /*! + * Project a single laser scan from a linear array into a 3D + * point cloud. The generated cloud will be in the same frame + * as the original laser scan. + * + * \param scan_in The input laser scan + * \param cloud_out The output point cloud + * \param range_cutoff An additional range cutoff which can be + * applied to discard everything above it. + * Defaults to -1.0, which means the laser scan max range. + * \param channel_option An OR'd set of channels to include. + * Options include: channel_option::Default, + * channel_option::Intensity, channel_option::Index, + * channel_option::Distance, channel_option::Timestamp. + */ + LASER_GEOMETRY_PUBLIC + void projectLaser( + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + double range_cutoff = -1.0, + int channel_options = channel_option::Default) + { + projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); + } + + // Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 in target frame + /*! + * Transform a single laser scan from a linear array into a 3D + * point cloud, accounting for movement of the laser over the + * course of the scan. In order for this transform to be + * meaningful at a single point in time, the target_frame must + * be a fixed reference frame. See the tf documentation for + * more information on fixed frames. + * + * \param target_frame The frame of the resulting point cloud + * \param scan_in The input laser scan + * \param cloud_out The output point cloud + * \param tf a tf2::BufferCore object to use to perform the + * transform + * \param range_cutoff An additional range cutoff which can be + * applied to discard everything above it. + * Defaults to -1.0, which means the laser scan max range. + * \param channel_option An OR'd set of channels to include. + * Options include: channel_option::Default, + * channel_option::Intensity, channel_option::Index, + * channel_option::Distance, channel_option::Timestamp. + */ + LASER_GEOMETRY_PUBLIC + void transformLaserScanToPointCloud( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + tf2::BufferCore & tf, + double range_cutoff = -1.0, + int channel_options = channel_option::Default) + { + transformLaserScanToPointCloud_( + target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); + } + +private: + // Internal hidden representation of projectLaser + void projectLaser_( + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + double range_cutoff, + int channel_options); + + // Internal hidden representation of transformLaserScanToPointCloud2 + void transformLaserScanToPointCloud_( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + tf2::BufferCore & tf, + double range_cutoff, + int channel_options); + + // Function used by the several forms of transformLaserScanToPointCloud_ + void transformLaserScanToPointCloud_( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + tf2::Quaternion quat_start, + tf2::Vector3 origin_start, + tf2::Quaternion quat_end, + tf2::Vector3 origin_end, + double range_cutoff, + int channel_options); + + // Internal map of pointers to stored values + float angle_min_; + float angle_max_; + Eigen::ArrayXXd co_sine_map_; +}; + +} // namespace laser_geometry + +#endif // LASER_GEOMETRY__LASER_GEOMETRY_HPP_ diff --git a/include/laser_geometry/visibility_control.hpp b/include/laser_geometry/visibility_control.hpp new file mode 100644 index 00000000..10e1e7c7 --- /dev/null +++ b/include/laser_geometry/visibility_control.hpp @@ -0,0 +1,64 @@ +// 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_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define LASER_GEOMETRY_EXPORT __attribute__ ((dllexport)) + #define LASER_GEOMETRY_IMPORT __attribute__ ((dllimport)) + #else + #define LASER_GEOMETRY_EXPORT __declspec(dllexport) + #define LASER_GEOMETRY_IMPORT __declspec(dllimport) + #endif + #ifdef LASER_GEOMETRY_BUILDING_LIBRARY + #define LASER_GEOMETRY_PUBLIC LASER_GEOMETRY_EXPORT + #else + #define LASER_GEOMETRY_PUBLIC LASER_GEOMETRY_IMPORT + #endif + #define LASER_GEOMETRY_PUBLIC_TYPE LASER_GEOMETRY_PUBLIC + #define LASER_GEOMETRY_LOCAL +#else + #define LASER_GEOMETRY_EXPORT __attribute__ ((visibility("default"))) + #define LASER_GEOMETRY_IMPORT + #if __GNUC__ >= 4 + #define LASER_GEOMETRY_PUBLIC __attribute__ ((visibility("default"))) + #define LASER_GEOMETRY_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define LASER_GEOMETRY_PUBLIC + #define LASER_GEOMETRY_LOCAL + #endif + #define LASER_GEOMETRY_PUBLIC_TYPE +#endif + +#endif // LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/package.xml b/package.xml index e9413883..6fdc1ab8 100644 --- a/package.xml +++ b/package.xml @@ -1,38 +1,53 @@ - + laser_geometry - 1.6.4 + 2.8.1 This package contains a class for converting from a 2D laser scan as defined by sensor_msgs/LaserScan into a point cloud as defined by sensor_msgs/PointCloud or sensor_msgs/PointCloud2. In particular, it contains functionality to account for the skew resulting from moving robots or tilting laser scanners. - Dave Hershberger - William Woodall + + Dharini Dutia + BSD - Tully Foote - Radu Bogdan Rusu + http://github.com/ros-perception/laser_geometry - http://ros.org/wiki/laser_geometry + Dave Hershberger + Mabel Zhang + Radu Bogdan Rusu + Tully Foote + William Woodall ament_cmake + ament_cmake_python + eigen3_cmake_module + + eigen3_cmake_module - angles - ecl_eigen + eigen rclcpp sensor_msgs tf2 - angles - ecl_eigen + eigen + + python3-numpy rclcpp + rclpy sensor_msgs + sensor_msgs_py tf2 - - ament_cmake + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + ament_cmake_pytest ament_cmake diff --git a/pytest.ini b/pytest.ini new file mode 100644 index 00000000..eded8a3c --- /dev/null +++ b/pytest.ini @@ -0,0 +1,3 @@ +; Needed to suppress warnings from pytest about the default junit_family +[pytest] +junit_family=xunit2 diff --git a/setup.py b/setup.py deleted file mode 100644 index 44edf6d0..00000000 --- a/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['laser_geometry'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index e6d4f9a3..4aff297b 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -1,615 +1,464 @@ -/* - * Copyright (c) 2008, Willow Garage, 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 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. - */ - -#include "laser_geometry/laser_geometry.h" -#include -#include "rclcpp/time.hpp" -#define TIME rclcpp::Time +// 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" + +#include -#define POINT_FIELD sensor_msgs::msg::PointField +#include +#include -// TODO: fix definitions -typedef double tfScalar; +#include "rclcpp/time.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" -#include +#include "tf2/LinearMath/Transform.h" namespace laser_geometry { +void LaserProjection::projectLaser_( + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + double range_cutoff, + int channel_options) +{ + size_t n_pts = scan_in.ranges.size(); + Eigen::ArrayXXd ranges(n_pts, 2); + Eigen::ArrayXXd output(n_pts, 2); + + // Get the ranges into Eigen format + for (size_t i = 0; i < n_pts; ++i) { + ranges(i, 0) = static_cast(scan_in.ranges[i]); + ranges(i, 1) = static_cast(scan_in.ranges[i]); + } - void - LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, sensor_msgs::msg::PointCloud & cloud_out, double range_cutoff, - bool preservative, int mask) + // Check if our existing co_sine_map is valid + if (co_sine_map_.rows() != static_cast(n_pts) || angle_min_ != scan_in.angle_min || + angle_max_ != scan_in.angle_max) { - boost::numeric::ublas::matrix ranges(2, scan_in.ranges.size()); - - // Fill the ranges matrix - for (unsigned int index = 0; index < scan_in.ranges.size(); index++) - { - ranges(0,index) = (double) scan_in.ranges[index]; - ranges(1,index) = (double) scan_in.ranges[index]; - } - - //Do the projection - // NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment)); - boost::numeric::ublas::matrix output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.ranges.size())); - - //Stuff the output cloud - cloud_out.header = scan_in.header; - cloud_out.points.resize (scan_in.ranges.size()); - - // Define 4 indices in the channel array for each possible value type - int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1; - - cloud_out.channels.resize(0); - - // Check if the intensity bit is set - if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0) - { - int chan_size = cloud_out.channels.size(); - cloud_out.channels.resize (chan_size + 1); - cloud_out.channels[0].name = "intensities"; - cloud_out.channels[0].values.resize (scan_in.intensities.size()); - idx_intensity = 0; - } - - // Check if the index bit is set - if (mask & channel_option::Index) - { - int chan_size = cloud_out.channels.size(); - cloud_out.channels.resize (chan_size +1); - cloud_out.channels[chan_size].name = "index"; - cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); - idx_index = chan_size; + // ROS_DEBUG("[projectLaser] No precomputed map given. Computing one."); + co_sine_map_ = Eigen::ArrayXXd(n_pts, 2); + angle_min_ = scan_in.angle_min; + angle_max_ = scan_in.angle_max; + // Spherical->Cartesian projection + for (size_t i = 0; i < n_pts; ++i) { + co_sine_map_(i, 0) = + cos(scan_in.angle_min + static_cast(i) * scan_in.angle_increment); + co_sine_map_(i, 1) = + sin(scan_in.angle_min + static_cast(i) * scan_in.angle_increment); } + } - // Check if the distance bit is set - if (mask & channel_option::Distance) - { - int chan_size = cloud_out.channels.size(); - cloud_out.channels.resize (chan_size + 1); - cloud_out.channels[chan_size].name = "distances"; - cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); - idx_distance = chan_size; - } + output = ranges * co_sine_map_; + + // Set the output cloud accordingly + cloud_out.header = scan_in.header; + cloud_out.height = 1; + cloud_out.width = static_cast(scan_in.ranges.size()); + cloud_out.fields.resize(3); + cloud_out.fields[0].name = "x"; + cloud_out.fields[0].offset = 0; + cloud_out.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[0].count = 1; + cloud_out.fields[1].name = "y"; + cloud_out.fields[1].offset = 4; + cloud_out.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[1].count = 1; + cloud_out.fields[2].name = "z"; + cloud_out.fields[2].offset = 8; + cloud_out.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[2].count = 1; + + // Define 4 indices in the channel array for each possible value type + int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, + idx_vpy = -1, idx_vpz = -1; + + // now, we need to check what fields we need to store + uint32_t offset = 12; + if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) { + size_t field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 1); + cloud_out.fields[field_size].name = "intensity"; + cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + offset += 4; + idx_intensity = static_cast(field_size); + } - if (mask & channel_option::Timestamp) - { - int chan_size = cloud_out.channels.size(); - cloud_out.channels.resize (chan_size + 1); - cloud_out.channels[chan_size].name = "stamps"; - cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); - idx_timestamp = chan_size; - } + if ((channel_options & channel_option::Index)) { + size_t field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 1); + cloud_out.fields[field_size].name = "index"; + cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::INT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + offset += 4; + idx_index = static_cast(field_size); + } - if (range_cutoff < 0) - range_cutoff = scan_in.range_max; + if ((channel_options & channel_option::Distance)) { + size_t field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 1); + cloud_out.fields[field_size].name = "distances"; + cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + offset += 4; + idx_distance = static_cast(field_size); + } - unsigned int count = 0; - for (unsigned int index = 0; index< scan_in.ranges.size(); index++) - { - const float range = ranges(0, index); - if (preservative || ((range < range_cutoff) && (range >= scan_in.range_min))) //if valid or preservative - { - cloud_out.points[count].x = output(0,index); - cloud_out.points[count].y = output(1,index); - cloud_out.points[count].z = 0.0; - - //double x = cloud_out.points[count].x; - //double y = cloud_out.points[count].y; - //if(x*x + y*y < scan_in.range_min * scan_in.range_min){ - // ROS_INFO("(%.2f, %.2f)", cloud_out.points[count].x, cloud_out.points[count].y); - //} - - // Save the original point index - if (idx_index != -1) - cloud_out.channels[idx_index].values[count] = index; - - // Save the original point distance - if (idx_distance != -1) - cloud_out.channels[idx_distance].values[count] = range; - - // Save intensities channel - if (scan_in.intensities.size() >= index) - { /// \todo optimize and catch length difference better - if (idx_intensity != -1) - cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index]; - } + if ((channel_options & channel_option::Timestamp)) { + size_t field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 1); + cloud_out.fields[field_size].name = "stamps"; + cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + offset += 4; + idx_timestamp = static_cast(field_size); + } - // Save timestamps to seperate channel if asked for - if( idx_timestamp != -1) - cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment; + if ((channel_options & channel_option::Viewpoint)) { + size_t field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 3); + + cloud_out.fields[field_size].name = "vp_x"; + cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + offset += 4; + + cloud_out.fields[field_size + 1].name = "vp_y"; + cloud_out.fields[field_size + 1].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[field_size + 1].offset = offset; + cloud_out.fields[field_size + 1].count = 1; + offset += 4; + + cloud_out.fields[field_size + 2].name = "vp_z"; + cloud_out.fields[field_size + 2].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud_out.fields[field_size + 2].offset = offset; + cloud_out.fields[field_size + 2].count = 1; + offset += 4; + + idx_vpx = static_cast(field_size); + idx_vpy = static_cast(field_size + 1); + idx_vpz = static_cast(field_size + 2); + } - count++; - } - } + cloud_out.point_step = offset; + cloud_out.row_step = cloud_out.point_step * cloud_out.width; + cloud_out.data.resize(cloud_out.row_step * cloud_out.height); + cloud_out.is_dense = false; - //downsize if necessary - cloud_out.points.resize (count); - for (unsigned int d = 0; d < cloud_out.channels.size(); d++) - cloud_out.channels[d].values.resize(count); - }; + if (range_cutoff < 0) { + range_cutoff = scan_in.range_max; + } -const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length) - { - boost::mutex::scoped_lock guv_lock(this->guv_mutex_); - - //construct string for lookup in the map - std::stringstream anglestring; - anglestring <* >::iterator it; - it = unit_vector_map_.find(anglestring.str()); - //check the map for presense - if (it != unit_vector_map_.end()) - return *((*it).second); //if present return - - boost::numeric::ublas::matrix * tempPtr = new boost::numeric::ublas::matrix(2,length); - for (unsigned int index = 0;index < length; index++) - { - (*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment); - (*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment); + unsigned int count = 0; + for (size_t i = 0; i < n_pts; ++i) { + // check to see if we want to keep the point + const float range = scan_in.ranges[i]; + if (range < range_cutoff && range >= scan_in.range_min) { + auto pstep = reinterpret_cast(&cloud_out.data[count * cloud_out.point_step]); + + // Copy XYZ + pstep[0] = static_cast(output(i, 0)); + pstep[1] = static_cast(output(i, 1)); + pstep[2] = 0; + + // Copy intensity + if (idx_intensity != -1) { + pstep[idx_intensity] = scan_in.intensities[i]; } - //store - unit_vector_map_[anglestring.str()] = tempPtr; - //and return - return *tempPtr; - }; - - LaserProjection::~LaserProjection() - { - std::map*>::iterator it; - it = unit_vector_map_.begin(); - while (it != unit_vector_map_.end()) - { - delete (*it).second; - it++; + // Copy index + if (idx_index != -1) { + reinterpret_cast(pstep)[idx_index] = static_cast(i); } - }; - void LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - double range_cutoff, - int channel_options) - { - size_t n_pts = scan_in.ranges.size (); - Eigen::ArrayXXd ranges (n_pts, 2); - Eigen::ArrayXXd output (n_pts, 2); - - // Get the ranges into Eigen format - for (size_t i = 0; i < n_pts; ++i) - { - ranges (i, 0) = (double) scan_in.ranges[i]; - ranges (i, 1) = (double) scan_in.ranges[i]; - } - - // Check if our existing co_sine_map is valid - if (co_sine_map_.rows () != (int)n_pts || angle_min_ != scan_in.angle_min || angle_max_ != scan_in.angle_max ) - { - ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one."); - co_sine_map_ = Eigen::ArrayXXd (n_pts, 2); - angle_min_ = scan_in.angle_min; - angle_max_ = scan_in.angle_max; - // Spherical->Cartesian projection - for (size_t i = 0; i < n_pts; ++i) - { - co_sine_map_ (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment); - co_sine_map_ (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment); + // Copy distance + if (idx_distance != -1) { + pstep[idx_distance] = range; } - } - output = ranges * co_sine_map_; - - // Set the output cloud accordingly - cloud_out.header = scan_in.header; - cloud_out.height = 1; - cloud_out.width = scan_in.ranges.size (); - cloud_out.fields.resize (3); - cloud_out.fields[0].name = "x"; - cloud_out.fields[0].offset = 0; - cloud_out.fields[0].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[0].count = 1; - cloud_out.fields[1].name = "y"; - cloud_out.fields[1].offset = 4; - cloud_out.fields[1].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[1].count = 1; - cloud_out.fields[2].name = "z"; - cloud_out.fields[2].offset = 8; - cloud_out.fields[2].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[2].count = 1; - - // Define 4 indices in the channel array for each possible value type - int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1; - - //now, we need to check what fields we need to store - int offset = 12; - if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) - { - int field_size = cloud_out.fields.size(); - cloud_out.fields.resize(field_size + 1); - cloud_out.fields[field_size].name = "intensity"; - cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - offset += 4; - idx_intensity = field_size; - } - - if ((channel_options & channel_option::Index)) - { - int field_size = cloud_out.fields.size(); - cloud_out.fields.resize(field_size + 1); - cloud_out.fields[field_size].name = "index"; - cloud_out.fields[field_size].datatype = POINT_FIELD::INT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - offset += 4; - idx_index = field_size; - } - - if ((channel_options & channel_option::Distance)) - { - int field_size = cloud_out.fields.size(); - cloud_out.fields.resize(field_size + 1); - cloud_out.fields[field_size].name = "distances"; - cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - offset += 4; - idx_distance = field_size; - } + // Copy timestamp + if (idx_timestamp != -1) { + pstep[idx_timestamp] = i * scan_in.time_increment; + } - if ((channel_options & channel_option::Timestamp)) - { - int field_size = cloud_out.fields.size(); - cloud_out.fields.resize(field_size + 1); - cloud_out.fields[field_size].name = "stamps"; - cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - offset += 4; - idx_timestamp = field_size; - } + // Copy viewpoint (0, 0, 0) + if (idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1) { + pstep[idx_vpx] = 0; + pstep[idx_vpy] = 0; + pstep[idx_vpz] = 0; + } - if ((channel_options & channel_option::Viewpoint)) - { - int field_size = cloud_out.fields.size(); - cloud_out.fields.resize(field_size + 3); - - cloud_out.fields[field_size].name = "vp_x"; - cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - offset += 4; - - cloud_out.fields[field_size + 1].name = "vp_y"; - cloud_out.fields[field_size + 1].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size + 1].offset = offset; - cloud_out.fields[field_size + 1].count = 1; - offset += 4; - - cloud_out.fields[field_size + 2].name = "vp_z"; - cloud_out.fields[field_size + 2].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size + 2].offset = offset; - cloud_out.fields[field_size + 2].count = 1; - offset += 4; - - idx_vpx = field_size; - idx_vpy = field_size + 1; - idx_vpz = field_size + 2; + // make sure to increment count + ++count; } - cloud_out.point_step = offset; - cloud_out.row_step = cloud_out.point_step * cloud_out.width; - cloud_out.data.resize (cloud_out.row_step * cloud_out.height); - cloud_out.is_dense = false; - - if (range_cutoff < 0) - range_cutoff = scan_in.range_max; - - unsigned int count = 0; - for (size_t i = 0; i < n_pts; ++i) + /* TODO(anonymous): Why was this done in this way, I don't get this at all, you end up with a + * ton of points with NaN values why can't you just leave them out? + * + // Invalid measurement? + if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min) { - //check to see if we want to keep the point - const float range = scan_in.ranges[i]; - if (range < range_cutoff && range >= scan_in.range_min) + if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE) { - float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step]; - - // Copy XYZ - pstep[0] = output (i, 0); - pstep[1] = output (i, 1); - pstep[2] = 0; - - // Copy intensity - if(idx_intensity != -1) - pstep[idx_intensity] = scan_in.intensities[i]; - - //Copy index - if(idx_index != -1) - ((int*)(pstep))[idx_index] = i; - - // Copy distance - if(idx_distance != -1) - pstep[idx_distance] = range; - - // Copy timestamp - if(idx_timestamp != -1) - pstep[idx_timestamp] = i * scan_in.time_increment; - - // Copy viewpoint (0, 0, 0) - if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1) - { - pstep[idx_vpx] = 0; - pstep[idx_vpy] = 0; - pstep[idx_vpz] = 0; - } - - //make sure to increment count - ++count; + for (size_t s = 0; s < cloud_out.fields.size (); ++s) + pstep[s] = bad_point; } - - /* TODO: Why was this done in this way, I don't get this at all, you end up with a ton of points with NaN values - * why can't you just leave them out? - * - // Invalid measurement? - if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min) + else { - if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE) + // Kind of nasty thing: + // We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid. + // Since we still might need the x value we store it in the distance field + pstep[0] = bad_point; // X -> NAN to mark a bad point + pstep[1] = co_sine_map (i, 1); // Y + pstep[2] = 0; // Z + + if (store_intensity) { - for (size_t s = 0; s < cloud_out.fields.size (); ++s) - pstep[s] = bad_point; + pstep[3] = bad_point; // Intensity -> NAN to mark a bad point + pstep[4] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X } else - { - // Kind of nasty thing: - // We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid. - // Since we still might need the x value we store it in the distance field - pstep[0] = bad_point; // X -> NAN to mark a bad point - pstep[1] = co_sine_map (i, 1); // Y - pstep[2] = 0; // Z - - if (store_intensity) - { - pstep[3] = bad_point; // Intensity -> NAN to mark a bad point - pstep[4] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X - } - else - pstep[3] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X - } + pstep[3] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X } - */ } + */ + } - //resize if necessary - cloud_out.width = count; - cloud_out.row_step = cloud_out.point_step * cloud_out.width; - cloud_out.data.resize (cloud_out.row_step * cloud_out.height); + // resize if necessary + cloud_out.width = count; + cloud_out.row_step = cloud_out.point_step * cloud_out.width; + cloud_out.data.resize(cloud_out.row_step * cloud_out.height); +} + +void LaserProjection::transformLaserScanToPointCloud_( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + tf2::Quaternion quat_start, + tf2::Vector3 origin_start, + tf2::Quaternion quat_end, + tf2::Vector3 origin_end, + double range_cutoff, + int channel_options) +{ + // check if the user has requested the index field + bool requested_index = false; + if ((channel_options & channel_option::Index)) { + requested_index = true; } - void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame, - const sensor_msgs::msg::LaserScan &scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - tf2::Quaternion quat_start, - tf2::Vector3 origin_start, - tf2::Quaternion quat_end, - tf2::Vector3 origin_end, - double range_cutoff, - int channel_options) - { - //check if the user has requested the index field - bool requested_index = false; - if ((channel_options & channel_option::Index)) - requested_index = true; - - //we'll enforce that we get index values for the laser scan so that we - //ensure that we use the correct timestamps - channel_options |= channel_option::Index; - - projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); - - //we'll assume no associated viewpoint by default - bool has_viewpoint = false; - uint32_t vp_x_offset = 0; - - //we need to find the offset of the intensity field in the point cloud - //we also know that the index field is guaranteed to exist since we - //set the channel option above. To be really safe, it might be worth - //putting in a check at some point, but I'm just going to put in an - //assert for now - uint32_t index_offset = 0; - for(unsigned int i = 0; i < cloud_out.fields.size(); ++i) - { - if(cloud_out.fields[i].name == "index") - { - index_offset = cloud_out.fields[i].offset; - } + // we'll enforce that we get index values for the laser scan so that we + // ensure that we use the correct timestamps + channel_options |= channel_option::Index; + + projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); + + // we'll assume no associated viewpoint by default + bool has_viewpoint = false; + uint32_t vp_x_offset = 0; + + // we need to find the offset of the intensity field in the point cloud + // we also know that the index field is guaranteed to exist since we + // set the channel option above. To be really safe, it might be worth + // putting in a check at some point, but I'm just going to put in an + // assert for now + uint32_t index_offset = 0; + for (unsigned int i = 0; i < cloud_out.fields.size(); ++i) { + if (cloud_out.fields[i].name == "index") { + index_offset = cloud_out.fields[i].offset; + } - //we want to check if the cloud has a viewpoint associated with it - //checking vp_x should be sufficient since vp_x, vp_y, and vp_z all - //get put in together - if(cloud_out.fields[i].name == "vp_x") - { - has_viewpoint = true; - vp_x_offset = cloud_out.fields[i].offset; - } + // we want to check if the cloud has a viewpoint associated with it + // checking vp_x should be sufficient since vp_x, vp_y, and vp_z all + // get put in together + if (cloud_out.fields[i].name == "vp_x") { + has_viewpoint = true; + vp_x_offset = cloud_out.fields[i].offset; } + } - ROS_ASSERT(index_offset > 0); + assert(index_offset > 0); - cloud_out.header.frame_id = target_frame; + cloud_out.header.frame_id = target_frame; - tf2::Transform cur_transform ; + tf2::Transform cur_transform; - double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0); + double ranges_norm = 1 / (static_cast(scan_in.ranges.size()) - 1.0); - //we want to loop through all the points in the cloud - for(size_t i = 0; i < cloud_out.width; ++i) - { - // Apply the transform to the current point - float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0]; + // we want to loop through all the points in the cloud + for (size_t i = 0; i < cloud_out.width; ++i) { + // Apply the transform to the current point + float * pstep = reinterpret_cast(&cloud_out.data[i * cloud_out.point_step + 0]); - //find the index of the point - uint32_t pt_index; - memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t)); + // find the index of the point + uint32_t pt_index; + memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t)); - // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms - tfScalar ratio = pt_index * ranges_norm; + // Assume constant motion during the laser-scan and use slerp to compute intermediate transforms + double ratio = pt_index * ranges_norm; - //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector) - // Interpolate translation - tf2::Vector3 v (0, 0, 0); - v.setInterpolate3 (origin_start, origin_end, ratio); - cur_transform.setOrigin (v); + // TODO(anon): Make a function that performs both the slerp and linear interpolation needed to + // interpolate a Full Transform (Quaternion + Vector) + // Interpolate translation + tf2::Vector3 v(0, 0, 0); + v.setInterpolate3(origin_start, origin_end, ratio); + cur_transform.setOrigin(v); - // Compute the slerp-ed rotation - cur_transform.setRotation (slerp (quat_start, quat_end , ratio)); + // Compute the slerp-ed rotation + cur_transform.setRotation(slerp(quat_start, quat_end, ratio)); - tf2::Vector3 point_in (pstep[0], pstep[1], pstep[2]); - tf2::Vector3 point_out = cur_transform * point_in; + tf2::Vector3 point_in(pstep[0], pstep[1], pstep[2]); + tf2::Vector3 point_out = cur_transform * point_in; - // Copy transformed point into cloud - pstep[0] = point_out.x (); - pstep[1] = point_out.y (); - pstep[2] = point_out.z (); + // Copy transformed point into cloud + pstep[0] = static_cast(point_out.x()); + pstep[1] = static_cast(point_out.y()); + pstep[2] = static_cast(point_out.z()); - // Convert the viewpoint as well - if(has_viewpoint) - { - float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset]; - point_in = tf2::Vector3 (vpstep[0], vpstep[1], vpstep[2]); - point_out = cur_transform * point_in; - - // Copy transformed point into cloud - vpstep[0] = point_out.x (); - vpstep[1] = point_out.y (); - vpstep[2] = point_out.z (); - } + // Convert the viewpoint as well + if (has_viewpoint) { + auto vpstep = + reinterpret_cast(&cloud_out.data[i * cloud_out.point_step + vp_x_offset]); + point_in = tf2::Vector3(vpstep[0], vpstep[1], vpstep[2]); + point_out = cur_transform * point_in; + + // Copy transformed point into cloud + vpstep[0] = static_cast(point_out.x()); + vpstep[1] = static_cast(point_out.y()); + vpstep[2] = static_cast(point_out.z()); } + } - //if the user didn't request the index field, then we need to copy the PointCloud and drop it - if(!requested_index) - { - sensor_msgs::msg::PointCloud2 cloud_without_index; - - //copy basic meta data - cloud_without_index.header = cloud_out.header; - cloud_without_index.width = cloud_out.width; - cloud_without_index.height = cloud_out.height; - cloud_without_index.is_bigendian = cloud_out.is_bigendian; - cloud_without_index.is_dense = cloud_out.is_dense; - - //copy the fields - cloud_without_index.fields.resize(cloud_out.fields.size()); - unsigned int field_count = 0; - unsigned int offset_shift = 0; - for(unsigned int i = 0; i < cloud_out.fields.size(); ++i) - { - if(cloud_out.fields[i].name != "index") - { - cloud_without_index.fields[field_count] = cloud_out.fields[i]; - cloud_without_index.fields[field_count].offset -= offset_shift; - ++field_count; - } - else - { - //once we hit the index, we'll set the shift - offset_shift = 4; - } + // if the user didn't request the index field, then we need to copy the PointCloud and drop it + if (!requested_index) { + sensor_msgs::msg::PointCloud2 cloud_without_index; + + // copy basic meta data + cloud_without_index.header = cloud_out.header; + cloud_without_index.width = cloud_out.width; + cloud_without_index.height = cloud_out.height; + cloud_without_index.is_bigendian = cloud_out.is_bigendian; + cloud_without_index.is_dense = cloud_out.is_dense; + + // copy the fields + cloud_without_index.fields.resize(cloud_out.fields.size()); + unsigned int field_count = 0; + unsigned int offset_shift = 0; + for (unsigned int i = 0; i < cloud_out.fields.size(); ++i) { + if (cloud_out.fields[i].name != "index") { + cloud_without_index.fields[field_count] = cloud_out.fields[i]; + cloud_without_index.fields[field_count].offset -= offset_shift; + ++field_count; + } else { + // once we hit the index, we'll set the shift + offset_shift = 4; } + } - //resize the fields - cloud_without_index.fields.resize(field_count); + // resize the fields + cloud_without_index.fields.resize(field_count); - //compute the size of the new data - cloud_without_index.point_step = cloud_out.point_step - offset_shift; - cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width; - cloud_without_index.data.resize (cloud_without_index.row_step * cloud_without_index.height); + // compute the size of the new data + cloud_without_index.point_step = cloud_out.point_step - offset_shift; + cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width; + cloud_without_index.data.resize(cloud_without_index.row_step * cloud_without_index.height); - uint32_t i = 0; - uint32_t j = 0; - //copy over the data from one cloud to the other - while (i < cloud_out.data.size()) + uint32_t i = 0; + uint32_t j = 0; + // copy over the data from one cloud to the other + while (i < cloud_out.data.size()) { + if ((i % cloud_out.point_step) < index_offset || + (i % cloud_out.point_step) >= (index_offset + 4)) { - if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4)) - { - cloud_without_index.data[j++] = cloud_out.data[i]; - } - i++; + cloud_without_index.data[j++] = cloud_out.data[i]; } - - //make sure to actually set the output - cloud_out = cloud_without_index; + i++; } - } - void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::msg::LaserScan &scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - tf2::BufferCore &tf, - double range_cutoff, - int channel_options) - { - TIME start_time = scan_in.header.stamp; - TIME end_time = scan_in.header.stamp; - // TODO: reconcile all the different time constructs - if (!scan_in.ranges.empty()) - { - end_time = end_time + rclcpp::Duration((scan_in.ranges.size() - 1) * scan_in.time_increment, 0); - } - - std::chrono::milliseconds start(start_time.nanoseconds()); - std::chrono::time_point st(start); - geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform(target_frame, scan_in.header.frame_id, st); - std::chrono::milliseconds end(end_time.nanoseconds()); - std::chrono::time_point e(end); - geometry_msgs::msg::TransformStamped end_transform = tf.lookupTransform(target_frame, scan_in.header.frame_id, e); - - tf2::Quaternion quat_start(start_transform.transform.rotation.x, - start_transform.transform.rotation.y, - start_transform.transform.rotation.z, - start_transform.transform.rotation.w); - tf2::Quaternion quat_end(end_transform.transform.rotation.x, - end_transform.transform.rotation.y, - end_transform.transform.rotation.z, - end_transform.transform.rotation.w); - - tf2::Vector3 origin_start(start_transform.transform.translation.x, - start_transform.transform.translation.y, - start_transform.transform.translation.z); - tf2::Vector3 origin_end(end_transform.transform.translation.x, - end_transform.transform.translation.y, - end_transform.transform.translation.z); - transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, - quat_start, origin_start, - quat_end, origin_end, - range_cutoff, - channel_options); + // make sure to actually set the output + cloud_out = cloud_without_index; + } +} + +void LaserProjection::transformLaserScanToPointCloud_( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + tf2::BufferCore & tf, + double range_cutoff, + int channel_options) +{ + rclcpp::Time start_time = scan_in.header.stamp; + rclcpp::Time end_time = scan_in.header.stamp; + // TODO(anonymous): reconcile all the different time constructs + if (!scan_in.ranges.empty()) { + end_time = start_time + rclcpp::Duration::from_seconds( + static_cast(scan_in.ranges.size() - 1) * static_cast(scan_in.time_increment)); } -} //laser_geometry + std::chrono::nanoseconds start(start_time.nanoseconds()); + std::chrono::time_point st(start); + geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform( + target_frame, scan_in.header.frame_id, st); + std::chrono::nanoseconds end(end_time.nanoseconds()); + std::chrono::time_point e(end); + geometry_msgs::msg::TransformStamped end_transform = tf.lookupTransform( + target_frame, scan_in.header.frame_id, e); + + tf2::Quaternion quat_start(start_transform.transform.rotation.x, + start_transform.transform.rotation.y, + start_transform.transform.rotation.z, + start_transform.transform.rotation.w); + tf2::Quaternion quat_end(end_transform.transform.rotation.x, + end_transform.transform.rotation.y, + end_transform.transform.rotation.z, + end_transform.transform.rotation.w); + + tf2::Vector3 origin_start(start_transform.transform.translation.x, + start_transform.transform.translation.y, + start_transform.transform.translation.z); + tf2::Vector3 origin_end(end_transform.transform.translation.x, + end_transform.transform.translation.y, + end_transform.transform.translation.z); + transformLaserScanToPointCloud_( + target_frame, scan_in, cloud_out, + quat_start, origin_start, + quat_end, origin_end, + range_cutoff, + channel_options); +} + +} // namespace laser_geometry 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 af2452ea..ef2dcde4 100644 --- a/src/laser_geometry/laser_geometry.py +++ b/src/laser_geometry/laser_geometry.py @@ -1,47 +1,48 @@ -""" -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. -""" - -import rospy -from sensor_msgs.msg import PointCloud2 -import sensor_msgs.point_cloud2 as pc2 +# 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 + class LaserProjection: """ - A class to Project Laser Scan + A class to Project Laser Scan. - This calls 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 your scanner is not changing) to avoid excess computation. - By default all range values less thatn the scanner min_range, + By default all range values less than the scanner min_range or greater than the scanner max_range are removed from the generated point cloud, as these are assumed to be invalid. @@ -62,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 @@ -81,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. @@ -90,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 @@ -105,13 +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): - rospy.logdebug("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)]) @@ -122,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 @@ -156,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 @@ -166,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 @@ -176,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 @@ -186,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 @@ -194,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 @@ -202,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 @@ -241,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 d664789b..3b7b4728 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -1,312 +1,264 @@ -/* - * Copyright (c) 2008, Willow Garage, 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 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 -#include - -#include "laser_geometry/laser_geometry.h" -#include "sensor_msgs/PointCloud.h" -#include +#include +#include +#include "rclcpp/rclcpp.hpp" -#include +#include "laser_geometry/laser_geometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" -#include "rostest/permuter.h" +#define PROJECTION_TEST_RANGE_MIN (0.23f) +#define PROJECTION_TEST_RANGE_MAX (40.0f) -#define PROJECTION_TEST_RANGE_MIN (0.23) -#define PROJECTION_TEST_RANGE_MAX (40.0) +#define PI static_cast(M_PI) +class BuildScanException +{ +}; -class BuildScanException { }; +struct ScanOptions +{ + float range_; + float intensity_; + float ang_min_; + float ang_max_; + float ang_increment_; + rclcpp::Duration scan_time_; + + ScanOptions( + float range, float intensity, + float ang_min, float ang_max, float ang_increment, + rclcpp::Duration scan_time) + : range_(range), + intensity_(intensity), + ang_min_(ang_min), + ang_max_(ang_max), + ang_increment_(ang_increment), + scan_time_(scan_time) {} +}; -sensor_msgs::msg::LaserScan build_constant_scan(double range, double intensity, - double ang_min, double ang_max, double ang_increment, - ros::Duration scan_time) +sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options) { - if (((ang_max - ang_min) / ang_increment) < 0) + if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) { throw (BuildScanException()); + } sensor_msgs::msg::LaserScan scan; - scan.header.stamp = ros::Time::now(); + scan.header.stamp = rclcpp::Clock().now(); scan.header.frame_id = "laser_frame"; - scan.angle_min = ang_min; - scan.angle_max = ang_max; - scan.angle_increment = ang_increment; - scan.scan_time = scan_time.toSec(); + scan.angle_min = options.ang_min_; + scan.angle_max = options.ang_max_; + scan.angle_increment = options.ang_increment_; + scan.scan_time = static_cast(options.scan_time_.nanoseconds()); scan.range_min = PROJECTION_TEST_RANGE_MIN; scan.range_max = PROJECTION_TEST_RANGE_MAX; uint32_t i = 0; - for(; ang_min + i * ang_increment < ang_max; i++) - { - scan.ranges.push_back(range); - scan.intensities.push_back(intensity); + for (; options.ang_min_ + i * options.ang_increment_ < options.ang_max_; i++) { + scan.ranges.push_back(options.range_); + scan.intensities.push_back(options.intensity_); } - scan.time_increment = scan_time.toSec()/(double)i; + scan.time_increment = + static_cast(options.scan_time_.nanoseconds() / static_cast(i)); return scan; -}; - +} -class TestProjection : public laser_geometry::LaserProjection +template +T cloudData(sensor_msgs::msg::PointCloud2 cloud_out, uint32_t index) { -public: - const boost::numeric::ublas::matrix& getUnitVectors(double angle_min, - double angle_max, - double angle_increment, - unsigned int length) - { - return getUnitVectors_(angle_min, angle_max, angle_increment, length); - } -}; + return *reinterpret_cast(&cloud_out.data[index]); +} -void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length) -{ +TEST(laser_geometry, projectLaser2) { double tolerance = 1e-12; - TestProjection projector; - - const boost::numeric::ublas::matrix & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length); - - - - for (unsigned int i = 0; i < mat.size2(); i++) - { - EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)), - angle_min + i * angle_increment), - 0, - tolerance); // check expected angle - EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance); //make sure normalized + laser_geometry::LaserProjection projector; + + std::vector ranges, intensities, min_angles, max_angles, angle_increments; + std::vector increment_times, scan_times; + + ranges.push_back(-1.0f); + ranges.push_back(1.0f); + ranges.push_back(2.0f); + ranges.push_back(100.0f); + + intensities.push_back(1.0f); + intensities.push_back(2.0f); + intensities.push_back(5.0f); + + min_angles.push_back(-PI); + min_angles.push_back(-PI / 1.5f); + min_angles.push_back(-PI / 8); + + max_angles.push_back(PI); + max_angles.push_back(PI / 1.5f); + max_angles.push_back(PI / 8); + + angle_increments.push_back(-PI / 180); // -one degree + angle_increments.push_back(PI / 180); // one degree + angle_increments.push_back(PI / 720); // quarter degree + + scan_times.push_back(rclcpp::Duration::from_seconds(1. / 40)); + scan_times.push_back(rclcpp::Duration::from_seconds(1. / 20)); + + std::vector options; + for (auto range : ranges) { + for (auto intensity : intensities) { + for (auto min_angle : min_angles) { + for (auto max_angle : max_angles) { + for (auto angle_increment : angle_increments) { + for (auto scan_time : scan_times) { + options.push_back( + ScanOptions( + range, intensity, min_angle, max_angle, angle_increment, scan_time)); + } + } + } + } + } } -} -#if 0 - -TEST(laser_geometry, getUnitVectors) -{ - double min_angle = -M_PI/2; - double max_angle = M_PI/2; - double angle_increment = M_PI/180; - - std::vector min_angles, max_angles, angle_increments; - - rostest::Permuter permuter; - min_angles.push_back(-M_PI); - min_angles.push_back(-M_PI/1.5); - min_angles.push_back(-M_PI/2); - min_angles.push_back(-M_PI/4); - min_angles.push_back(-M_PI/8); - min_angles.push_back(M_PI); - min_angles.push_back(M_PI/1.5); - min_angles.push_back(M_PI/2); - min_angles.push_back(M_PI/4); - min_angles.push_back(M_PI/8); - permuter.addOptionSet(min_angles, &min_angle); + for (auto option : options) { + try { + // printf("%f %f %f %f %f %f\n", + // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); + sensor_msgs::msg::LaserScan scan = build_constant_scan(option); + + sensor_msgs::msg::PointCloud2 cloud_out; + projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), 4u); + projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); + EXPECT_EQ(cloud_out.fields.size(), 4u); + + projector.projectLaser(scan, cloud_out, -1.0); + EXPECT_EQ(cloud_out.fields.size(), 5u); + projector.projectLaser( + scan, cloud_out, -1.0, + laser_geometry::channel_option::Intensity | + laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), 5u); + + projector.projectLaser( + scan, cloud_out, -1.0, + laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | + laser_geometry::channel_option::Distance); + EXPECT_EQ(cloud_out.fields.size(), 6u); + + projector.projectLaser( + scan, cloud_out, -1.0, + laser_geometry::channel_option::Intensity | + laser_geometry::channel_option::Index | + laser_geometry::channel_option::Distance | + laser_geometry::channel_option::Timestamp); + EXPECT_EQ(cloud_out.fields.size(), 7u); + + unsigned int valid_points = 0; + for (unsigned int i = 0; i < scan.ranges.size(); i++) { + if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && + scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) + { + valid_points++; + } + } - max_angles.push_back(M_PI); - max_angles.push_back(M_PI/1.5); - max_angles.push_back(M_PI/2); - max_angles.push_back(M_PI/4); - max_angles.push_back(M_PI/8); - max_angles.push_back(-M_PI); - max_angles.push_back(-M_PI/1.5); - max_angles.push_back(-M_PI/2); - max_angles.push_back(-M_PI/4); - max_angles.push_back(-M_PI/8); - permuter.addOptionSet(max_angles, &max_angle); - - angle_increments.push_back(M_PI/180); // one degree - angle_increments.push_back(M_PI/360); // half degree - angle_increments.push_back(M_PI/720); // quarter degree - angle_increments.push_back(-M_PI/180); // -one degree - angle_increments.push_back(-M_PI/360); // -half degree - angle_increments.push_back(-M_PI/720); // -quarter degree - permuter.addOptionSet(angle_increments, &angle_increment); - - - while (permuter.step()) - { - if ((max_angle - min_angle) / angle_increment > 0.0) - { - unsigned int length = round((max_angle - min_angle)/ angle_increment); - try + EXPECT_EQ(valid_points, cloud_out.width); + + uint32_t x_offset = 0; + uint32_t y_offset = 0; + uint32_t z_offset = 0; + uint32_t intensity_offset = 0; + uint32_t index_offset = 0; + uint32_t distance_offset = 0; + uint32_t stamps_offset = 0; + for (std::vector::iterator f = cloud_out.fields.begin(); + f != cloud_out.fields.end(); f++) { - test_getUnitVectors(min_angle, max_angle, angle_increment, length); - test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment); - test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1); + if (f->name == "x") {x_offset = f->offset;} + if (f->name == "y") {y_offset = f->offset;} + if (f->name == "z") {z_offset = f->offset;} + if (f->name == "intensity") {intensity_offset = f->offset;} + if (f->name == "index") {index_offset = f->offset;} + if (f->name == "distances") {distance_offset = f->offset;} + if (f->name == "stamps") {stamps_offset = f->offset;} } - catch (BuildScanException &ex) - { - if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception + + for (unsigned int i = 0; i < cloud_out.width; i++) { + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + x_offset), + static_cast(static_cast(scan.ranges[i]) * + cos(static_cast(scan.angle_min) + i * static_cast(scan.angle_increment))), + tolerance); + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + y_offset), + static_cast(static_cast(scan.ranges[i]) * + sin(static_cast(scan.angle_min) + i * static_cast(scan.angle_increment))), + tolerance); + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance); + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + intensity_offset), + scan.intensities[i], tolerance); // intensity + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + index_offset), i, + tolerance); // index + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + distance_offset), + scan.ranges[i], tolerance); // ranges + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + stamps_offset), + (float)i * scan.time_increment, tolerance); // timestamps + } + } catch (const BuildScanException & ex) { + (void) ex; + // make sure it is not a false exception + if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) { FAIL(); } } - //else - //printf("%f\n", (max_angle - min_angle) / angle_increment); } } +// TODO(Martin-Idel-SI): Reenable test if possible. Test fails due to lookupTransform failing +// Needs to publish a transform to "laser_frame" in order to work. +#if 0 +TEST(laser_geometry, transformLaserScanToPointCloud2) { + tf2::BufferCore tf2; -TEST(laser_geometry, projectLaser) -{ - double tolerance = 1e-12; - laser_geometry::LaserProjection projector; - - double min_angle = -M_PI/2; - double max_angle = M_PI/2; - double angle_increment = M_PI/180; - - double range = 1.0; - double intensity = 1.0; - - ros::Duration scan_time = ros::Duration(1/40); - ros::Duration increment_time = ros::Duration(1/400); - - - std::vector ranges, intensities, min_angles, max_angles, angle_increments; - std::vector increment_times, scan_times; - - rostest::Permuter permuter; - - ranges.push_back(-1.0); - ranges.push_back(1.0); - ranges.push_back(2.0); - ranges.push_back(3.0); - ranges.push_back(4.0); - ranges.push_back(5.0); - ranges.push_back(100.0); - permuter.addOptionSet(ranges, &range); - - intensities.push_back(1.0); - intensities.push_back(2.0); - intensities.push_back(3.0); - intensities.push_back(4.0); - intensities.push_back(5.0); - permuter.addOptionSet(intensities, &intensity); - - min_angles.push_back(-M_PI); - min_angles.push_back(-M_PI/1.5); - min_angles.push_back(-M_PI/2); - min_angles.push_back(-M_PI/4); - min_angles.push_back(-M_PI/8); - permuter.addOptionSet(min_angles, &min_angle); - - max_angles.push_back(M_PI); - max_angles.push_back(M_PI/1.5); - max_angles.push_back(M_PI/2); - max_angles.push_back(M_PI/4); - max_angles.push_back(M_PI/8); - permuter.addOptionSet(max_angles, &max_angle); - - // angle_increments.push_back(-M_PI/180); // -one degree - angle_increments.push_back(M_PI/180); // one degree - angle_increments.push_back(M_PI/360); // half degree - angle_increments.push_back(M_PI/720); // quarter degree - permuter.addOptionSet(angle_increments, &angle_increment); - - scan_times.push_back(ros::Duration(1/40)); - scan_times.push_back(ros::Duration(1/20)); - - permuter.addOptionSet(scan_times, &scan_time); - - while (permuter.step()) - { - try - { - // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::msg::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); - - sensor_msgs::msg::PointCloud cloud_out; - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); - - projector.projectLaser(scan, cloud_out, -1.0); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); - - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3); - - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4); - - unsigned int valid_points = 0; - for (unsigned int i = 0; i < scan.ranges.size(); i++) - if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) - valid_points ++; - - EXPECT_EQ(valid_points, cloud_out.points.size()); - - for (unsigned int i = 0; i < cloud_out.points.size(); i++) - { - EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance); - EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order - EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index - EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges - EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps - }; - } - catch (BuildScanException &ex) - { - if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception - FAIL(); - } - } -} - -#endif - - -TEST(laser_geometry, projectLaser2) -{ double tolerance = 1e-12; - laser_geometry::LaserProjection projector; - - double min_angle = -M_PI/2; - double max_angle = M_PI/2; - double angle_increment = M_PI/180; - - double range = 1.0; - double intensity = 1.0; - - ros::Duration scan_time = ros::Duration(1/40); - ros::Duration increment_time = ros::Duration(1/400); - + laser_geometry::LaserProjection projector; std::vector ranges, intensities, min_angles, max_angles, angle_increments; - std::vector increment_times, scan_times; - - rostest::Permuter permuter; + std::vector increment_times, scan_times; ranges.push_back(-1.0); ranges.push_back(1.0); @@ -315,385 +267,161 @@ TEST(laser_geometry, projectLaser2) ranges.push_back(4.0); ranges.push_back(5.0); ranges.push_back(100.0); - permuter.addOptionSet(ranges, &range); intensities.push_back(1.0); intensities.push_back(2.0); intensities.push_back(3.0); intensities.push_back(4.0); intensities.push_back(5.0); - permuter.addOptionSet(intensities, &intensity); min_angles.push_back(-M_PI); - min_angles.push_back(-M_PI/1.5); - min_angles.push_back(-M_PI/2); - min_angles.push_back(-M_PI/4); - min_angles.push_back(-M_PI/8); - permuter.addOptionSet(min_angles, &min_angle); + min_angles.push_back(-M_PI / 1.5); + min_angles.push_back(-M_PI / 2); + min_angles.push_back(-M_PI / 4); + min_angles.push_back(-M_PI / 8); max_angles.push_back(M_PI); - max_angles.push_back(M_PI/1.5); - max_angles.push_back(M_PI/2); - max_angles.push_back(M_PI/4); - max_angles.push_back(M_PI/8); - permuter.addOptionSet(max_angles, &max_angle); - - // angle_increments.push_back(-M_PI/180); // -one degree - angle_increments.push_back(M_PI/180); // one degree - angle_increments.push_back(M_PI/360); // half degree - angle_increments.push_back(M_PI/720); // quarter degree - permuter.addOptionSet(angle_increments, &angle_increment); - - scan_times.push_back(ros::Duration(1/40)); - scan_times.push_back(ros::Duration(1/20)); - - permuter.addOptionSet(scan_times, &scan_time); - - while (permuter.step()) - { - try - { - // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::msg::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); - - sensor_msgs::msg::PointCloud2 cloud_out; - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - - projector.projectLaser(scan, cloud_out, -1.0); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); - - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); - - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); - - unsigned int valid_points = 0; - for (unsigned int i = 0; i < scan.ranges.size(); i++) - if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) - valid_points ++; - - EXPECT_EQ(valid_points, cloud_out.width); - - uint32_t x_offset = 0; - uint32_t y_offset = 0; - uint32_t z_offset = 0; - uint32_t intensity_offset = 0; - uint32_t index_offset = 0; - uint32_t distance_offset = 0; - uint32_t stamps_offset = 0; - for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) - { - if (f->name == "x") x_offset = f->offset; - if (f->name == "y") y_offset = f->offset; - if (f->name == "z") z_offset = f->offset; - if (f->name == "intensity") intensity_offset = f->offset; - if (f->name == "index") index_offset = f->offset; - if (f->name == "distances") distance_offset = f->offset; - if (f->name == "stamps") stamps_offset = f->offset; - } - - for (unsigned int i = 0; i < cloud_out.width; i++) - { - - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order - EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps - }; - } - catch (BuildScanException &ex) - { - if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception - FAIL(); + max_angles.push_back(M_PI / 1.5); + max_angles.push_back(M_PI / 2); + max_angles.push_back(M_PI / 4); + max_angles.push_back(M_PI / 8); + + angle_increments.push_back(-M_PI / 180); // -one degree + angle_increments.push_back(M_PI / 180); // one degree + angle_increments.push_back(M_PI / 360); // half degree + angle_increments.push_back(M_PI / 720); // quarter degree + + scan_times.push_back(rclcpp::Duration::from_seconds(1. / 40)); + scan_times.push_back(rclcpp::Duration::from_seconds(1. / 20)); + + std::vector options; + for (auto range : ranges) { + for (auto intensity : intensities) { + for (auto min_angle : min_angles) { + for (auto max_angle : max_angles) { + for (auto angle_increment : angle_increments) { + for (auto scan_time : scan_times) { + options.push_back( + ScanOptions( + range, intensity, min_angle, max_angle, angle_increment, scan_time)); + } + } + } + } } } -} - -TEST(laser_geometry, transformLaserScanToPointCloud) -{ - - tf::Transformer tf; - - double tolerance = 1e-12; - laser_geometry::LaserProjection projector; - - double min_angle = -M_PI/2; - double max_angle = M_PI/2; - double angle_increment = M_PI/180; - - double range = 1.0; - double intensity = 1.0; - - ros::Duration scan_time = ros::Duration(1/40); - ros::Duration increment_time = ros::Duration(1/400); - - - std::vector ranges, intensities, min_angles, max_angles, angle_increments; - std::vector increment_times, scan_times; - - rostest::Permuter permuter; - - ranges.push_back(-1.0); - ranges.push_back(1.0); - ranges.push_back(2.0); - ranges.push_back(3.0); - ranges.push_back(4.0); - ranges.push_back(5.0); - ranges.push_back(100.0); - permuter.addOptionSet(ranges, &range); - - intensities.push_back(1.0); - intensities.push_back(2.0); - intensities.push_back(3.0); - intensities.push_back(4.0); - intensities.push_back(5.0); - permuter.addOptionSet(intensities, &intensity); - - min_angles.push_back(-M_PI); - min_angles.push_back(-M_PI/1.5); - min_angles.push_back(-M_PI/2); - min_angles.push_back(-M_PI/4); - min_angles.push_back(-M_PI/8); - - - max_angles.push_back(M_PI); - max_angles.push_back(M_PI/1.5); - max_angles.push_back(M_PI/2); - max_angles.push_back(M_PI/4); - max_angles.push_back(M_PI/8); - - permuter.addOptionSet(min_angles, &min_angle); - permuter.addOptionSet(max_angles, &max_angle); - - angle_increments.push_back(-M_PI/180); // -one degree - angle_increments.push_back(M_PI/180); // one degree - angle_increments.push_back(M_PI/360); // half degree - angle_increments.push_back(M_PI/720); // quarter degree - permuter.addOptionSet(angle_increments, &angle_increment); - - scan_times.push_back(ros::Duration(1/40)); - scan_times.push_back(ros::Duration(1/20)); - - permuter.addOptionSet(scan_times, &scan_time); - - while (permuter.step()) - { - try - { - //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::msg::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); - scan.header.frame_id = "laser_frame"; + for (auto option : options) { + try { + // printf("%f %f %f %f %f %f\n", + // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); + sensor_msgs::msg::LaserScan scan = build_constant_scan(option); + + scan.header.frame_id = "laser_frame"; + + sensor_msgs::msg::PointCloud2 cloud_out; + projector.transformLaserScanToPointCloud( + scan.header.frame_id, scan, cloud_out, tf2, -1.0, + laser_geometry::channel_option::None); + EXPECT_EQ(cloud_out.fields.size(), 3u); + projector.transformLaserScanToPointCloud( + scan.header.frame_id, scan, cloud_out, tf2, -1.0, + laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), 4u); + projector.transformLaserScanToPointCloud( + scan.header.frame_id, scan, cloud_out, tf2, -1.0, + laser_geometry::channel_option::Intensity); + EXPECT_EQ(cloud_out.fields.size(), 4u); + + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2); + EXPECT_EQ(cloud_out.fields.size(), 5u); + projector.transformLaserScanToPointCloud( + scan.header.frame_id, scan, cloud_out, tf2, -1.0, + laser_geometry::channel_option::Intensity | + laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), 5u); + + projector.transformLaserScanToPointCloud( + scan.header.frame_id, scan, cloud_out, tf2, -1.0, + laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | + laser_geometry::channel_option::Distance); + EXPECT_EQ(cloud_out.fields.size(), 6u); + + projector.transformLaserScanToPointCloud( + scan.header.frame_id, scan, cloud_out, tf2, -1.0, + laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | + laser_geometry::channel_option::Distance | + laser_geometry::channel_option::Timestamp); + EXPECT_EQ(cloud_out.fields.size(), 7u); + + EXPECT_EQ(cloud_out.is_dense, false); + + unsigned int valid_points = 0; + for (unsigned int i = 0; i < scan.ranges.size(); i++) { + if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && + scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) + { + valid_points++; + } + } + EXPECT_EQ(valid_points, cloud_out.width); + + uint32_t x_offset = 0; + uint32_t y_offset = 0; + uint32_t z_offset = 0; + uint32_t intensity_offset = 0; + uint32_t index_offset = 0; + uint32_t distance_offset = 0; + uint32_t stamps_offset = 0; + for (std::vector::iterator f = cloud_out.fields.begin(); + f != cloud_out.fields.end(); f++) + { + if (f->name == "x") {x_offset = f->offset;} + if (f->name == "y") {y_offset = f->offset;} + if (f->name == "z") {z_offset = f->offset;} + if (f->name == "intensity") {intensity_offset = f->offset;} + if (f->name == "index") {index_offset = f->offset;} + if (f->name == "distances") {distance_offset = f->offset;} + if (f->name == "stamps") {stamps_offset = f->offset;} + } - sensor_msgs::msg::PointCloud cloud_out; - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); - - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); - - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3); - - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp); - EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4); - - unsigned int valid_points = 0; - for (unsigned int i = 0; i < scan.ranges.size(); i++) - if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) - valid_points ++; - EXPECT_EQ(valid_points, cloud_out.points.size()); - - for (unsigned int i = 0; i < cloud_out.points.size(); i++) - { - EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance); - EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order - EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index - EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges - EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps - }; - } - catch (BuildScanException &ex) - { - if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception - FAIL(); + for (unsigned int i = 0; i < cloud_out.width; i++) { + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + x_offset), + static_cast(static_cast(scan.ranges[i]) * + cos(static_cast(scan.angle_min) + i * static_cast(scan.angle_increment))), + tolerance); + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + y_offset), + static_cast(static_cast(scan.ranges[i]) * + sin(static_cast(scan.angle_min) + i * static_cast(scan.angle_increment))), + tolerance); + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance); + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + intensity_offset), + scan.intensities[i], tolerance); // intensity + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + index_offset), i, + tolerance); // index + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + distance_offset), + scan.ranges[i], tolerance); // ranges + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + stamps_offset), + (float)i * scan.time_increment, tolerance); // timestamps + } + } catch (BuildScanException & ex) { + // make sure it is not a false exception + if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) { + FAIL(); + } } } } +#endif -TEST(laser_geometry, transformLaserScanToPointCloud2) +int main(int argc, char ** argv) { - - tf::Transformer tf; - tf2::BufferCore tf2; - - double tolerance = 1e-12; - laser_geometry::LaserProjection projector; - - double min_angle = -M_PI/2; - double max_angle = M_PI/2; - double angle_increment = M_PI/180; - - double range = 1.0; - double intensity = 1.0; - - ros::Duration scan_time = ros::Duration(1/40); - ros::Duration increment_time = ros::Duration(1/400); - - std::vector ranges, intensities, min_angles, max_angles, angle_increments; - std::vector increment_times, scan_times; - - rostest::Permuter permuter; - - ranges.push_back(-1.0); - ranges.push_back(1.0); - ranges.push_back(2.0); - ranges.push_back(3.0); - ranges.push_back(4.0); - ranges.push_back(5.0); - ranges.push_back(100.0); - permuter.addOptionSet(ranges, &range); - - intensities.push_back(1.0); - intensities.push_back(2.0); - intensities.push_back(3.0); - intensities.push_back(4.0); - intensities.push_back(5.0); - permuter.addOptionSet(intensities, &intensity); - - min_angles.push_back(-M_PI); - min_angles.push_back(-M_PI/1.5); - min_angles.push_back(-M_PI/2); - min_angles.push_back(-M_PI/4); - min_angles.push_back(-M_PI/8); - - - max_angles.push_back(M_PI); - max_angles.push_back(M_PI/1.5); - max_angles.push_back(M_PI/2); - max_angles.push_back(M_PI/4); - max_angles.push_back(M_PI/8); - - permuter.addOptionSet(min_angles, &min_angle); - permuter.addOptionSet(max_angles, &max_angle); - - angle_increments.push_back(-M_PI/180); // -one degree - angle_increments.push_back(M_PI/180); // one degree - angle_increments.push_back(M_PI/360); // half degree - angle_increments.push_back(M_PI/720); // quarter degree - permuter.addOptionSet(angle_increments, &angle_increment); - - scan_times.push_back(ros::Duration(1/40)); - scan_times.push_back(ros::Duration(1/20)); - - permuter.addOptionSet(scan_times, &scan_time); - - while (permuter.step()) - { - try - { - //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::msg::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); - scan.header.frame_id = "laser_frame"; - - sensor_msgs::msg::PointCloud2 cloud_out; - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); - - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); - - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); - - EXPECT_EQ(cloud_out.is_dense, false); - - unsigned int valid_points = 0; - for (unsigned int i = 0; i < scan.ranges.size(); i++) - if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) - valid_points ++; - EXPECT_EQ(valid_points, cloud_out.width); - - uint32_t x_offset = 0; - uint32_t y_offset = 0; - uint32_t z_offset = 0; - uint32_t intensity_offset = 0; - uint32_t index_offset = 0; - uint32_t distance_offset = 0; - uint32_t stamps_offset = 0; - for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) - { - if (f->name == "x") x_offset = f->offset; - if (f->name == "y") y_offset = f->offset; - if (f->name == "z") z_offset = f->offset; - if (f->name == "intensity") intensity_offset = f->offset; - if (f->name == "index") index_offset = f->offset; - if (f->name == "distances") distance_offset = f->offset; - if (f->name == "stamps") stamps_offset = f->offset; - } - - for (unsigned int i = 0; i < cloud_out.width; i++) - { - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order - EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps - - }; - } - catch (BuildScanException &ex) - { - if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception - FAIL(); - } - } - -} - - -int main(int argc, char **argv){ - ros::Time::init(); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/test/projection_test.py b/test/projection_test.py index e9da60c1..895e53d6 100755 --- a/test/projection_test.py +++ b/test/projection_test.py @@ -1,23 +1,51 @@ -#!/usr/bin/env python +# 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. -PKG='laser_geometry' +from itertools import product -import rospy -import sensor_msgs.point_cloud2 as pc2 -from sensor_msgs.msg import LaserScan from laser_geometry import LaserProjection - import numpy as np -from itertools import product - -import unittest +import pytest +import rclpy +import rclpy.duration +import rclpy.time +from sensor_msgs.msg import LaserScan +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: + +class BuildScanException(Exception): pass + def build_constant_scan( range_val, intensity_val, angle_min, angle_max, angle_increment, scan_time): @@ -26,138 +54,103 @@ def build_constant_scan( raise BuildScanException scan = LaserScan() - scan.header.stamp = rospy.rostime.Time.from_sec(10.10) - scan.header.frame_id = "laser_frame" + scan.header.stamp = rclpy.time.Time(seconds=10.10).to_msg() + scan.header.frame_id = 'laser_frame' scan.angle_min = angle_min scan.angle_max = angle_max scan.angle_increment = angle_increment - scan.scan_time = scan_time.to_sec() + scan.scan_time = scan_time.nanoseconds / 1e9 scan.range_min = PROJECTION_TEST_RANGE_MIN scan.range_max = PROJECTION_TEST_RANGE_MAX scan.ranges = [range_val for _ in range(count)] scan.intensities = [intensity_val for _ in range(count)] - scan.time_increment = scan_time.to_sec()/count + scan.time_increment = scan_time.nanoseconds / 1e9 / count return scan -class ProjectionTest(unittest.TestCase): - - def test_project_laser(self): - tolerance = 6 # decimal places - projector = LaserProjection() - - ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0] - intensities = np.arange(1.0, 6.0).tolist() - - min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0]) - max_angles = -min_angles - - angle_increments = np.pi / np.array([180., 360., 720.]) - - scan_times = [rospy.Duration(1./i) for i in [40, 20]] - - for range_val, intensity_val, \ - angle_min, angle_max, angle_increment, scan_time in \ - product(ranges, intensities, - min_angles, max_angles, angle_increments, scan_times): - try: - scan = build_constant_scan( - range_val, intensity_val, - angle_min, angle_max, angle_increment, scan_time) - except BuildScanException: - if (angle_max - angle_min)/angle_increment > 0: - self.fail() - - cloud_out = projector.projectLaser(scan, -1.0, - LaserProjection.ChannelOption.INDEX) - self.assertEquals(len(cloud_out.fields), 4, - "PointCloud2 with channel INDEX: fields size != 4") - - cloud_out = projector.projectLaser(scan, -1.0, - LaserProjection.ChannelOption.INTENSITY) - self.assertEquals(len(cloud_out.fields), 4, - "PointCloud2 with channel INDEX: fields size != 4") - - cloud_out = projector.projectLaser(scan, -1.0) - self.assertEquals(len(cloud_out.fields), 5, - "PointCloud2 with channel INDEX: fields size != 5") - cloud_out = projector.projectLaser(scan, -1.0, - LaserProjection.ChannelOption.INTENSITY | - LaserProjection.ChannelOption.INDEX) - self.assertEquals(len(cloud_out.fields), 5, - "PointCloud2 with channel INDEX: fields size != 5") - - cloud_out = projector.projectLaser(scan, -1.0, - LaserProjection.ChannelOption.INTENSITY | - LaserProjection.ChannelOption.INDEX | - LaserProjection.ChannelOption.DISTANCE) - self.assertEquals(len(cloud_out.fields), 6, - "PointCloud2 with channel INDEX: fields size != 6") - - cloud_out = projector.projectLaser(scan, -1.0, - LaserProjection.ChannelOption.INTENSITY | - LaserProjection.ChannelOption.INDEX | - LaserProjection.ChannelOption.DISTANCE | - LaserProjection.ChannelOption.TIMESTAMP) - self.assertEquals(len(cloud_out.fields), 7, - "PointCloud2 with channel INDEX: fields size != 7") - - 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): - valid_points += 1 - - self.assertEqual(valid_points, cloud_out.width, - "Valid points != PointCloud2 width") - - idx_x = idx_y = idx_z = 0 - idx_intensity = idx_index = 0 - idx_distance = idx_stamps = 0 - - i = 0 - for f in cloud_out.fields: - if f.name == "x": - idx_x = i - elif f.name == "y": - idx_y = i - elif f.name == "z": - idx_z = i - elif f.name == "intensity": - idx_intensity = i - elif f.name == "index": - idx_index = i - elif f.name == "distances": - idx_distance = i - elif f.name == "stamps": - idx_stamps = i - i += 1 - - i = 0 - for point in pc2.read_points(cloud_out): - ri = scan.ranges[i] - ai = scan.angle_min + i * scan.angle_increment - - self.assertAlmostEqual(point[idx_x], ri * np.cos(ai), - tolerance, "x not equal") - self.assertAlmostEqual(point[idx_y], ri * np.sin(ai), - tolerance, "y not equal") - self.assertAlmostEqual(point[idx_z], 0, - tolerance, "z not equal") - self.assertAlmostEqual(point[idx_intensity], - scan.intensities[i], - tolerance, "Intensity not equal") - self.assertAlmostEqual(point[idx_index], i, - tolerance, "Index not equal") - self.assertAlmostEqual(point[idx_distance], ri, - tolerance, "Distance not equal") - self.assertAlmostEqual(point[idx_stamps], - i * scan.time_increment, - tolerance, "Timestamp not equal") - i += 1 + +def test_project_laser(): + tolerance = 6 # decimal places + projector = LaserProjection() + + ranges = [-1.0, 1.0, 5.0, 100.0] + intensities = np.arange(1.0, 4.0).tolist() + + min_angles = -np.pi / np.array([1.0, 1.5, 8.0]) + max_angles = -min_angles + + angle_increments = np.pi / np.array([180., 360., 720.]) + + scan_times = [rclpy.duration.Duration(seconds=1./i) for i in [40, 20]] + + for range_val, intensity_val, \ + angle_min, angle_max, angle_increment, scan_time in \ + product( + ranges, intensities, + min_angles, max_angles, + angle_increments, scan_times): + try: + scan = build_constant_scan( + range_val, intensity_val, + angle_min, angle_max, angle_increment, scan_time) + except BuildScanException: + assert (angle_max - angle_min)/angle_increment <= 0 + + 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): + valid_points += 1 + + assert valid_points == cloud_out.width, 'Valid points != PointCloud2 width' + + idx_x = idx_y = idx_z = 0 + idx_intensity = idx_index = 0 + idx_distance = idx_stamps = 0 + + i = 0 + for f in cloud_out.fields: + if f.name == 'x': + idx_x = i + elif f.name == 'y': + idx_y = i + elif f.name == 'z': + idx_z = i + elif f.name == 'intensity': + idx_intensity = i + elif f.name == 'index': + idx_index = i + elif f.name == 'distances': + idx_distance = i + elif f.name == 'stamps': + idx_stamps = i + i += 1 + + i = 0 + for point in pc2.read_points(cloud_out): + 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_stamps] == pytest.approx( + i * scan.time_increment, tolerance), 'Timestamp not equal' + i += 1 if __name__ == '__main__': - import rosunit - rosunit.unitrun(PKG, 'projection_test', ProjectionTest) + pytest.main()