hiep update
This commit is contained in:
205
include/robot_laser_geometry/laser_geometry.hpp
Normal file
205
include/robot_laser_geometry/laser_geometry.hpp
Normal file
@@ -0,0 +1,205 @@
|
||||
// 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_
|
||||
Reference in New Issue
Block a user