206 lines
8.3 KiB
C++
206 lines
8.3 KiB
C++
// 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 ROBOT_LASER_GEOMETRY_ROBOT_LASER_GEOMETRY_HPP_
|
|
#define ROBOT_LASER_GEOMETRY_ROBOT_LASER_GEOMETRY_HPP_
|
|
|
|
#include <map>
|
|
#include <iostream>
|
|
#include <sstream>
|
|
#include <string>
|
|
|
|
#include <Eigen/Core> // NOLINT (cpplint cannot handle include order here)
|
|
|
|
#include "tf3/buffer_core.h"
|
|
#include <robot_sensor_msgs/LaserScan.h>
|
|
#include <robot_sensor_msgs/PointCloud2.h>
|
|
#include <robot_geometry_msgs/TransformStamped.h>
|
|
#include "robot_laser_geometry/visibility_control.hpp"
|
|
|
|
namespace robot_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/robot_laser_geometry/issues/29
|
|
|
|
class LaserProjection
|
|
{
|
|
public:
|
|
LaserProjection()
|
|
: angle_min_(0), angle_max_(0) {}
|
|
|
|
// Project a robot_sensor_msgs::LaserScan into a robot_sensor_msgs::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.
|
|
*/
|
|
ROBOT_LASER_GEOMETRY_PUBLIC
|
|
void projectLaser(
|
|
const robot_sensor_msgs::LaserScan & scan_in,
|
|
robot_sensor_msgs::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 robot_sensor_msgs::LaserScan into a robot_sensor_msgs::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 tf3::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.
|
|
*/
|
|
ROBOT_LASER_GEOMETRY_PUBLIC
|
|
void transformLaserScanToPointCloud(
|
|
const std::string & target_frame,
|
|
const robot_sensor_msgs::LaserScan & scan_in,
|
|
robot_sensor_msgs::PointCloud2 & cloud_out,
|
|
tf3::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 robot_sensor_msgs::LaserScan & scan_in,
|
|
robot_sensor_msgs::PointCloud2 & cloud_out,
|
|
double range_cutoff,
|
|
int channel_options);
|
|
|
|
// Internal hidden representation of transformLaserScanToPointCloud2
|
|
void transformLaserScanToPointCloud_(
|
|
const std::string & target_frame,
|
|
const robot_sensor_msgs::LaserScan & scan_in,
|
|
robot_sensor_msgs::PointCloud2 & cloud_out,
|
|
tf3::BufferCore & tf,
|
|
double range_cutoff,
|
|
int channel_options);
|
|
|
|
// Function used by the several forms of transformLaserScanToPointCloud_
|
|
void transformLaserScanToPointCloud_(
|
|
const std::string & target_frame,
|
|
const robot_sensor_msgs::LaserScan & scan_in,
|
|
robot_sensor_msgs::PointCloud2 & cloud_out,
|
|
tf3::Quaternion quat_start,
|
|
tf3::Vector3 origin_start,
|
|
tf3::Quaternion quat_end,
|
|
tf3::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 robot_laser_geometry
|
|
|
|
#endif // ROBOT_LASER_GEOMETRY__ROBOT_LASER_GEOMETRY_HPP_
|