.. _program_listing_file__tmp_ws_src_laser_geometry_include_laser_geometry_laser_geometry.hpp: Program Listing for File laser_geometry.hpp =========================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/laser_geometry/include/laser_geometry/laser_geometry.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /* * 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 #include #include #include #include // NOLINT (cpplint cannot handle include order here) #include "tf2/buffer_core.h" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "laser_geometry/visibility_control.hpp" namespace laser_geometry { // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle) const float LASER_SCAN_INVALID = -1.0; const float LASER_SCAN_MIN_RANGE = -2.0; const float LASER_SCAN_MAX_RANGE = -3.0; namespace channel_option { // Enumerated output channels options. 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_