laser_geometry.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #ifndef LASER_SCAN_UTILS_LASERSCAN_H
00031 #define LASER_SCAN_UTILS_LASERSCAN_H
00032 
00033 #include <map>
00034 #include <iostream>
00035 #include <sstream>
00036 
00037 #include "boost/numeric/ublas/matrix.hpp"
00038 #include "boost/thread/mutex.hpp"
00039 
00040 #include <tf/tf.h>
00041 #include <tf2/buffer_core.h>
00042 
00043 #include "sensor_msgs/LaserScan.h"
00044 #include "sensor_msgs/PointCloud.h"
00045 #include "sensor_msgs/PointCloud.h"
00046 
00047 #include <Eigen/Core>
00048 #include <sensor_msgs/PointCloud2.h>
00049 
00050 #include <ros/macros.h>
00051 
00052 // Import/export for windows dll's and visibility for gcc shared libraries.
00053 
00054 #ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
00055   #ifdef laser_geometry_EXPORTS // we are building a shared lib/dll
00056     #define LASER_GEOMETRY_DECL ROS_HELPER_EXPORT
00057   #else // we are using shared lib/dll
00058     #define LASER_GEOMETRY_DECL ROS_HELPER_IMPORT
00059   #endif
00060 #else // ros is being built around static libraries
00061   #define LASER_GEOMETRY_DECL
00062 #endif
00063 
00064 namespace laser_geometry
00065 {
00066   // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle)
00067   const float LASER_SCAN_INVALID   = -1.0;
00068   const float LASER_SCAN_MIN_RANGE = -2.0;
00069   const float LASER_SCAN_MAX_RANGE = -3.0;
00070 
00071   namespace channel_option
00072   {
00074 
00079     enum ChannelOption
00080       {
00081         None = 0x00,      
00082         Intensity = 0x01, 
00083         Index     = 0x02, 
00084         Distance  = 0x04, 
00085         Timestamp = 0x08, 
00086         Viewpoint = 0x10, 
00087         Default   = (Intensity | Index) 
00088       };
00089   }
00090 
00092 
00113   class LASER_GEOMETRY_DECL LaserProjection
00114     {
00115 
00116     public:
00117 
00118       LaserProjection() : angle_min_(0), angle_max_(0) {}
00119 
00121       ~LaserProjection();
00122 
00124 
00139       void projectLaser (const sensor_msgs::LaserScan& scan_in,
00140                          sensor_msgs::PointCloud& cloud_out,
00141                          double range_cutoff = -1.0,
00142                          int channel_options = channel_option::Default)
00143       {
00144         return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options);
00145       }
00146 
00148 
00163       void projectLaser (const sensor_msgs::LaserScan& scan_in,
00164                          sensor_msgs::PointCloud2 &cloud_out,
00165                          double range_cutoff = -1.0,
00166                          int channel_options = channel_option::Default)
00167       {
00168         projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
00169       }
00170 
00171 
00173 
00193       void transformLaserScanToPointCloud (const std::string& target_frame,
00194                                            const sensor_msgs::LaserScan& scan_in,
00195                                            sensor_msgs::PointCloud& cloud_out,
00196                                            tf::Transformer& tf,
00197                                            double range_cutoff,
00198                                            int channel_options = channel_option::Default)
00199       {
00200         return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options);
00201       }
00202 
00204 
00222       void transformLaserScanToPointCloud (const std::string& target_frame,
00223                                            const sensor_msgs::LaserScan& scan_in,
00224                                            sensor_msgs::PointCloud& cloud_out,
00225                                            tf::Transformer& tf,
00226                                            int channel_options = channel_option::Default)
00227       {
00228         return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options);
00229       }
00230 
00232 
00253       void transformLaserScanToPointCloud(const std::string &target_frame,
00254                                            const sensor_msgs::LaserScan &scan_in,
00255                                            sensor_msgs::PointCloud2 &cloud_out,
00256                                            tf::Transformer &tf,
00257                                            double range_cutoff = -1.0,
00258                                            int channel_options = channel_option::Default)
00259       {
00260         transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
00261       }
00262 
00264 
00285       void transformLaserScanToPointCloud(const std::string &target_frame,
00286                                           const sensor_msgs::LaserScan &scan_in,
00287                                           sensor_msgs::PointCloud2 &cloud_out,
00288                                           tf2::BufferCore &tf,
00289                                           double range_cutoff = -1.0,
00290                                           int channel_options = channel_option::Default)
00291       {
00292         transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
00293       }
00294 
00295     protected:
00296 
00298 
00303       const boost::numeric::ublas::matrix<double>& getUnitVectors_(double angle_min,
00304                                                                    double angle_max,
00305                                                                    double angle_increment,
00306                                                                    unsigned int length);
00307 
00308     private:
00309 
00311       void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
00312                           sensor_msgs::PointCloud& cloud_out,
00313                           double range_cutoff,
00314                           bool preservative,
00315                           int channel_options);
00316 
00318       void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
00319                           sensor_msgs::PointCloud2 &cloud_out,
00320                           double range_cutoff,
00321                           int channel_options);
00322 
00324       void transformLaserScanToPointCloud_ (const std::string& target_frame,
00325                                             sensor_msgs::PointCloud& cloud_out,
00326                                             const sensor_msgs::LaserScan& scan_in,
00327                                             tf::Transformer & tf,
00328                                             double range_cutoff,
00329                                             int channel_options);
00330 
00332       void transformLaserScanToPointCloud_ (const std::string &target_frame,
00333                                             const sensor_msgs::LaserScan &scan_in,
00334                                             sensor_msgs::PointCloud2 &cloud_out,
00335                                             tf::Transformer &tf,
00336                                             double range_cutoff,
00337                                             int channel_options);
00338 
00340       void transformLaserScanToPointCloud_ (const std::string &target_frame,
00341                                             const sensor_msgs::LaserScan &scan_in,
00342                                             sensor_msgs::PointCloud2 &cloud_out,
00343                                             tf2::BufferCore &tf,
00344                                             double range_cutoff,
00345                                             int channel_options);
00346 
00348       void transformLaserScanToPointCloud_ (const std::string &target_frame,
00349                                             const sensor_msgs::LaserScan &scan_in,
00350                                             sensor_msgs::PointCloud2 &cloud_out,
00351                                             tf2::Quaternion quat_start,
00352                                             tf2::Vector3 origin_start,
00353                                             tf2::Quaternion quat_end,
00354                                             tf2::Vector3 origin_end,
00355                                             double range_cutoff,
00356                                             int channel_options);
00357 
00359       std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
00360       float angle_min_;
00361       float angle_max_;
00362       Eigen::ArrayXXd co_sine_map_;
00363       boost::mutex guv_mutex_;
00364     };
00365 
00366 }
00367 
00368 #endif //LASER_SCAN_UTILS_LASERSCAN_H


laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Tue Jul 2 2019 20:04:25