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

Merge pull request #28 from bsinno/feature/refactoring_for_linux

Browse Source 
Polish ros2 branch
This commit is contained in:
Jonathan Binney 2018-03-28 11:29:20 -07:00 committed by GitHub
commit 0edeab0703
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 967 additions and 1487 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
CMakeLists.txt
View File

@ -2,44 +2,76 @@ cmake_minimum_required(VERSION 3.5)
project(laser_geometry)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(ament_cmake REQUIRED)
find_package(angles REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(Eigen3 REQUIRED)
set(Boost_USE_STATIC_LIBS ON)
find_package(Boost REQUIRED)
# TODO(dhood): enable python support once ported to ROS 2
# catkin_python_setup()
include_directories(include
${angles_INCLUDE_DIRS}
${rclcpp_INCLUDE_DIRS}
${sensor_msgs_INCLUDE_DIRS}
${tf2_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)
add_library(laser_geometry src/laser_geometry.cpp)
add_library(laser_geometry SHARED src/laser_geometry.cpp)
target_link_libraries(laser_geometry
${angles_LIBRARIES}
${rclcpp_LIBRARIES}
${sensor_msgs_LIBRARIES}
${tf2_LIBRARIES}
)
ament_export_include_directories(include)
ament_export_libraries(laser_geometry)
ament_package(CONFIG_EXTRAS laser_geometry-extras.cmake)
# 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")
install(TARGETS laser_geometry
ament_export_include_directories(include)
ament_export_interfaces(laser_geometry)
ament_export_libraries(laser_geometry)
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
)
if(BUILD_TESTING)
# TODO(Martin-Idel-SI): replace this with ament_lint_auto() and/or add the copyright linter back
find_package(ament_cmake_cppcheck REQUIRED)
find_package(ament_cmake_cpplint REQUIRED)
find_package(ament_cmake_lint_cmake REQUIRED)
find_package(ament_cmake_uncrustify REQUIRED)
ament_cppcheck()
ament_cpplint()
ament_lint_cmake()
ament_uncrustify()
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_gmock REQUIRED)
ament_add_gtest(projection_test
test/projection_test.cpp
TIMEOUT 180)
if(TARGET projection_test)
target_link_libraries(projection_test laser_geometry)
endif()
endif()
ament_package(CONFIG_EXTRAS laser_geometry-extras.cmake)

317
include/laser_geometry/laser_geometry.h
View File

@ -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 <map>
#include <iostream>
#include <sstream>
#include "boost/numeric/ublas/matrix.hpp"
#include "boost/thread/mutex.hpp"
#include <tf2/buffer_core.h>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#ifndef ROS_DEBUG
#define ROS_DEBUG(...)
#endif // !ROS_DEBUG
#ifndef ROS_ASSERT
#define ROS_ASSERT(...)
#endif // !ROS_ASSERT
#include <Eigen/Core>
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<double>& 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<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
float angle_min_;
float angle_max_;
Eigen::ArrayXXd co_sine_map_;
boost::mutex guv_mutex_;
};
}
#endif //LASER_SCAN_UTILS_LASERSCAN_H

203
include/laser_geometry/laser_geometry.hpp Normal file
View File

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

9
package.xml
View File

@ -20,13 +20,11 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>angles</build_depend>
<build_depend>Eigen3</build_depend>
<build_depend>rclcpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf2</build_depend>
<exec_depend>angles</exec_depend>
<exec_depend>Eigen3</exec_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
@ -34,6 +32,13 @@
<exec_depend>ament_cmake</exec_depend>
<test_depend>ament_cmake_cppcheck</test_depend>
<test_depend>ament_cmake_cpplint</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_cmake_lint_cmake</test_depend>
<test_depend>ament_cmake_uncrustify</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>

362
src/laser_geometry.cpp
View File

@ -1,5 +1,6 @@
/*
* 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
@ -27,172 +28,25 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "laser_geometry/laser_geometry.h"
#include "laser_geometry/laser_geometry.hpp"
#include <algorithm>
#include <string>
#include "rclcpp/time.hpp"
#define TIME rclcpp::Time
#define POINT_FIELD sensor_msgs::msg::PointField
// TODO: fix definitions
typedef double tfScalar;
#include <tf2/LinearMath/Transform.h>
#include "tf2/LinearMath/Transform.h"
namespace laser_geometry
{
void
LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, sensor_msgs::msg::PointCloud & cloud_out, double range_cutoff,
bool preservative, int mask)
{
boost::numeric::ublas::matrix<double> 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<double> 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;
}
// 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;
}
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 (range_cutoff < 0)
range_cutoff = scan_in.range_max;
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];
}
// 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;
count++;
}
}
//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);
};
const boost::numeric::ublas::matrix<double>& 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 <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;
std::map<std::string, boost::numeric::ublas::matrix<double>* >::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<double> * tempPtr = new boost::numeric::ublas::matrix<double>(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);
}
//store
unit_vector_map_[anglestring.str()] = tempPtr;
//and return
return *tempPtr;
};
LaserProjection::~LaserProjection()
{
std::map<std::string, boost::numeric::ublas::matrix<double>*>::iterator it;
it = unit_vector_map_.begin();
while (it != unit_vector_map_.end())
{
delete (*it).second;
it++;
}
};
void LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in,
void LaserProjection::projectLaser_(
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
double range_cutoff,
int channel_options)
@ -202,24 +56,25 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
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];
for (size_t i = 0; i < n_pts; ++i) {
ranges(i, 0) = static_cast<double>(scan_in.ranges[i]);
ranges(i, 1) = static_cast<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 )
if (co_sine_map_.rows() != static_cast<int>(n_pts) || angle_min_ != scan_in.angle_min ||
angle_max_ != scan_in.angle_max)
{
ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one.");
// 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);
for (size_t i = 0; i < n_pts; ++i) {
co_sine_map_(i, 0) =
cos(scan_in.angle_min + static_cast<double>(i) * scan_in.angle_increment);
co_sine_map_(i, 1) =
sin(scan_in.angle_min + static_cast<double>(i) * scan_in.angle_increment);
}
}
@ -228,7 +83,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
// Set the output cloud accordingly
cloud_out.header = scan_in.header;
cloud_out.height = 1;
cloud_out.width = scan_in.ranges.size ();
cloud_out.width = static_cast<uint32_t>(scan_in.ranges.size());
cloud_out.fields.resize(3);
cloud_out.fields[0].name = "x";
cloud_out.fields[0].offset = 0;
@ -244,61 +99,57 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
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;
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();
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 = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_intensity = field_size;
idx_intensity = static_cast<int>(field_size);
}
if ((channel_options & channel_option::Index))
{
int field_size = cloud_out.fields.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 = POINT_FIELD::INT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_index = field_size;
idx_index = static_cast<int>(field_size);
}
if ((channel_options & channel_option::Distance))
{
int field_size = cloud_out.fields.size();
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 = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_distance = field_size;
idx_distance = static_cast<int>(field_size);
}
if ((channel_options & channel_option::Timestamp))
{
int field_size = cloud_out.fields.size();
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 = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_timestamp = field_size;
idx_timestamp = static_cast<int>(field_size);
}
if ((channel_options & channel_option::Viewpoint))
{
int field_size = cloud_out.fields.size();
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";
@ -319,9 +170,9 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.fields[field_size + 2].count = 1;
offset += 4;
idx_vpx = field_size;
idx_vpy = field_size + 1;
idx_vpz = field_size + 2;
idx_vpx = static_cast<int>(field_size);
idx_vpy = static_cast<int>(field_size + 1);
idx_vpz = static_cast<int>(field_size + 2);
}
cloud_out.point_step = offset;
@ -329,42 +180,44 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.data.resize(cloud_out.row_step * cloud_out.height);
cloud_out.is_dense = false;
if (range_cutoff < 0)
if (range_cutoff < 0) {
range_cutoff = scan_in.range_max;
}
unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i)
{
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)
{
float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
if (range < range_cutoff && range >= scan_in.range_min) {
auto pstep = reinterpret_cast<float *>(&cloud_out.data[count * cloud_out.point_step]);
// Copy XYZ
pstep[0] = output (i, 0);
pstep[1] = output (i, 1);
pstep[0] = static_cast<float>(output(i, 0));
pstep[1] = static_cast<float>(output(i, 1));
pstep[2] = 0;
// Copy intensity
if(idx_intensity != -1)
if (idx_intensity != -1) {
pstep[idx_intensity] = scan_in.intensities[i];
}
// Copy index
if(idx_index != -1)
((int*)(pstep))[idx_index] = i;
if (idx_index != -1) {
reinterpret_cast<int *>(pstep)[idx_index] = static_cast<int>(i);
}
// Copy distance
if(idx_distance != -1)
if (idx_distance != -1) {
pstep[idx_distance] = range;
}
// Copy timestamp
if(idx_timestamp != -1)
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)
{
if (idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1) {
pstep[idx_vpx] = 0;
pstep[idx_vpy] = 0;
pstep[idx_vpz] = 0;
@ -374,8 +227,8 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
++count;
}
/* 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?
/* 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)
@ -412,7 +265,8 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.data.resize(cloud_out.row_step * cloud_out.height);
}
void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame,
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,
@ -424,8 +278,9 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
{
// check if the user has requested the index field
bool requested_index = false;
if ((channel_options & channel_option::Index))
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
@ -443,45 +298,42 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
// 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")
{
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")
{
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;
tf2::Transform cur_transform;
double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0);
double ranges_norm = 1 / (static_cast<double>(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)
{
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];
float * pstep = reinterpret_cast<float *>(&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));
// Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
// Assume constant motion during the laser-scan and use slerp to compute intermediate transforms
tfScalar 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)
// 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);
@ -494,27 +346,26 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
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 ();
pstep[0] = static_cast<float>(point_out.x());
pstep[1] = static_cast<float>(point_out.y());
pstep[2] = static_cast<float>(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];
if (has_viewpoint) {
auto vpstep =
reinterpret_cast<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 ();
vpstep[0] = static_cast<float>(point_out.x());
vpstep[1] = static_cast<float>(point_out.y());
vpstep[2] = static_cast<float>(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)
{
if (!requested_index) {
sensor_msgs::msg::PointCloud2 cloud_without_index;
// copy basic meta data
@ -528,16 +379,12 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
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")
{
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
{
} else {
// once we hit the index, we'll set the shift
offset_shift = 4;
}
@ -554,9 +401,9 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
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))
while (i < cloud_out.data.size()) {
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];
}
@ -568,7 +415,8 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
}
}
void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame,
void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
tf2::BufferCore & tf,
@ -577,18 +425,22 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
{
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);
// TODO(anonymous): reconcile all the different time constructs
if (!scan_in.ranges.empty()) {
end_time = end_time + rclcpp::Duration(
static_cast<int>((scan_in.ranges.size() - 1) * scan_in.time_increment), 0);
}
std::chrono::milliseconds start(start_time.nanoseconds());
std::chrono::time_point<std::chrono::system_clock> 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<std::chrono::system_clock> e(end);
geometry_msgs::msg::TransformStamped end_transform = tf.lookupTransform(target_frame, scan_in.header.frame_id, e);
std::chrono::nanoseconds start(start_time.nanoseconds());
std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> 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<std::chrono::system_clock, std::chrono::nanoseconds> 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,
@ -612,4 +464,4 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
channel_options);
}
} //laser_geometry
} // namespace laser_geometry

761
test/projection_test.cpp
View File

@ -1,5 +1,6 @@
/*
* 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
@ -27,356 +28,165 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#define _USE_MATH_DEFINES
#include <gtest/gtest.h>
#include <sys/time.h>
#include <cmath>
#include <vector>
#include "laser_geometry/laser_geometry.h"
#include "sensor_msgs/PointCloud.h"
#include <math.h>
#include "rclcpp/rclcpp.hpp"
#include "laser_geometry/laser_geometry.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <angles/angles.h>
#define PROJECTION_TEST_RANGE_MIN (0.23f)
#define PROJECTION_TEST_RANGE_MAX (40.0f)
#include "rostest/permuter.h"
#define PI static_cast<float>(M_PI)
#define PROJECTION_TEST_RANGE_MIN (0.23)
#define PROJECTION_TEST_RANGE_MAX (40.0)
class BuildScanException { };
sensor_msgs::msg::LaserScan build_constant_scan(double range, double intensity,
double ang_min, double ang_max, double ang_increment,
ros::Duration scan_time)
class BuildScanException
{
if (((ang_max - ang_min) / ang_increment) < 0)
};
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(const ScanOptions & options)
{
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<float>(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<float>(options.scan_time_.nanoseconds() / static_cast<double>(i));
return scan;
};
class TestProjection : public laser_geometry::LaserProjection
{
public:
const boost::numeric::ublas::matrix<double>& getUnitVectors(double angle_min,
double angle_max,
double angle_increment,
unsigned int length)
{
return getUnitVectors_(angle_min, angle_max, angle_increment, length);
}
};
void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
{
double tolerance = 1e-12;
TestProjection projector;
const boost::numeric::ublas::matrix<double> & 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
}
}
#if 0
TEST(laser_geometry, getUnitVectors)
template<typename T>
T cloudData(sensor_msgs::msg::PointCloud2 cloud_out, uint32_t index)
{
double min_angle = -M_PI/2;
double max_angle = M_PI/2;
double angle_increment = M_PI/180;
std::vector<double> 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);
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
{
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);
}
catch (BuildScanException &ex)
{
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
FAIL();
}
}
//else
//printf("%f\n", (max_angle - min_angle) / angle_increment);
}
return *reinterpret_cast<T *>(&cloud_out.data[index]);
}
TEST(laser_geometry, projectLaser)
{
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;
std::vector<float> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<rclcpp::Duration> increment_times, scan_times;
double range = 1.0;
double intensity = 1.0;
ranges.push_back(-1.0f);
ranges.push_back(1.0f);
ranges.push_back(2.0f);
ranges.push_back(100.0f);
ros::Duration scan_time = ros::Duration(1/40);
ros::Duration increment_time = ros::Duration(1/400);
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);
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<ros::Duration> increment_times, scan_times;
max_angles.push_back(PI);
max_angles.push_back(PI / 1.5f);
max_angles.push_back(PI / 8);
rostest::Permuter permuter;
angle_increments.push_back(-PI / 180); // -one degree
angle_increments.push_back(PI / 180); // one degree
angle_increments.push_back(PI / 720); // quarter degree
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);
scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20));
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
};
std::vector<ScanOptions> 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));
}
}
}
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);
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<ros::Duration> 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);
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(), (unsigned int)4);
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(), (unsigned int)4);
EXPECT_EQ(cloud_out.fields.size(), 4u);
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);
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(), (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);
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(), (unsigned int)7);
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)
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);
@ -387,178 +197,58 @@ TEST(laser_geometry, projectLaser2)
uint32_t index_offset = 0;
uint32_t distance_offset = 0;
uint32_t stamps_offset = 0;
for (std::vector<sensor_msgs::msg::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
for (std::vector<sensor_msgs::msg::PointField>::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;
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
};
for (unsigned int i = 0; i < cloud_out.width; i++) {
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) *
cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) *
sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + intensity_offset),
scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(cloudData<uint32_t>(cloud_out, i * cloud_out.point_step + index_offset), i,
tolerance); // index
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + distance_offset),
scan.ranges[i], tolerance); // ranges
EXPECT_NEAR(cloudData<float>(cloud_out, 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
} 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();
}
}
}
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<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<ros::Duration> 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::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();
}
}
}
TEST(laser_geometry, transformLaserScanToPointCloud2)
{
tf::Transformer tf;
// 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;
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<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<ros::Duration> increment_times, scan_times;
rostest::Permuter permuter;
std::vector<rclcpp::Duration> increment_times, scan_times;
ranges.push_back(-1.0);
ranges.push_back(1.0);
@ -567,14 +257,12 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
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);
@ -582,74 +270,83 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
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));
scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20));
permuter.addOptionSet(scan_times, &scan_time);
std::vector<ScanOptions> 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));
}
}
}
}
}
}
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);
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, 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, 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);
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, 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, 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, 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);
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)
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;
@ -659,41 +356,49 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
uint32_t index_offset = 0;
uint32_t distance_offset = 0;
uint32_t stamps_offset = 0;
for (std::vector<sensor_msgs::msg::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
for (std::vector<sensor_msgs::msg::PointField>::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;
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
};
for (unsigned int i = 0; i < cloud_out.width; i++) {
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) *
cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) *
sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + intensity_offset),
scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(cloudData<uint32_t>(cloud_out, i * cloud_out.point_step + index_offset), i,
tolerance); // index
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + distance_offset),
scan.ranges[i], tolerance); // ranges
EXPECT_NEAR(cloudData<float>(cloud_out, 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
} 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
int main(int argc, char **argv){
ros::Time::init();
int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}