Program Listing for File laser_geometry.hpp

↰ Return to documentation for file (/tmp/ws/src/laser_geometry/include/laser_geometry/laser_geometry.hpp)

/*
 * 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.
enum ChannelOption
{
  None = 0x00,
  Intensity = 0x01,
  Index     = 0x02,
  Distance  = 0x04,
  Timestamp = 0x08,
  Viewpoint = 0x10,
  Default   = (Intensity | Index)
};
}  // namespace channel_option


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