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 namespace laser_geometry
00051 {
00052   // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle)
00053   const float LASER_SCAN_INVALID   = -1.0;
00054   const float LASER_SCAN_MIN_RANGE = -2.0;
00055   const float LASER_SCAN_MAX_RANGE = -3.0;
00056 
00057   namespace channel_option
00058   {
00060 
00065     enum ChannelOption
00066       {
00067         None = 0x00,      
00068         Intensity = 0x01, 
00069         Index     = 0x02, 
00070         Distance  = 0x04, 
00071         Timestamp = 0x08, 
00072         Viewpoint = 0x10, 
00073         Default   = (Intensity | Index) 
00074       };
00075   }
00076 
00078 
00099   class LaserProjection
00100     {
00101 
00102     public:
00103 
00104       LaserProjection() : angle_min_(0), angle_max_(0) {}
00105 
00107       ~LaserProjection();
00108 
00110 
00125       void projectLaser (const sensor_msgs::LaserScan& scan_in,
00126                          sensor_msgs::PointCloud& cloud_out,
00127                          double range_cutoff = -1.0,
00128                          int channel_options = channel_option::Default)
00129       {
00130         return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options);
00131       }
00132 
00134 
00149       void projectLaser (const sensor_msgs::LaserScan& scan_in,
00150                          sensor_msgs::PointCloud2 &cloud_out,
00151                          double range_cutoff = -1.0,
00152                          int channel_options = channel_option::Default)
00153       {
00154         projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
00155       }
00156 
00157 
00159 
00179       void transformLaserScanToPointCloud (const std::string& target_frame,
00180                                            const sensor_msgs::LaserScan& scan_in,
00181                                            sensor_msgs::PointCloud& cloud_out,
00182                                            tf::Transformer& tf,
00183                                            double range_cutoff,
00184                                            int channel_options = channel_option::Default)
00185       {
00186         return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options);
00187       }
00188 
00190 
00208       void transformLaserScanToPointCloud (const std::string& target_frame,
00209                                            const sensor_msgs::LaserScan& scan_in,
00210                                            sensor_msgs::PointCloud& cloud_out,
00211                                            tf::Transformer& tf,
00212                                            int channel_options = channel_option::Default)
00213       {
00214         return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options);
00215       }
00216 
00218 
00239       void transformLaserScanToPointCloud(const std::string &target_frame,
00240                                            const sensor_msgs::LaserScan &scan_in,
00241                                            sensor_msgs::PointCloud2 &cloud_out,
00242                                            tf::Transformer &tf,
00243                                            double range_cutoff = -1.0,
00244                                            int channel_options = channel_option::Default)
00245       {
00246         transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
00247       }
00248 
00250 
00271       void transformLaserScanToPointCloud(const std::string &target_frame,
00272                                           const sensor_msgs::LaserScan &scan_in,
00273                                           sensor_msgs::PointCloud2 &cloud_out,
00274                                           tf2::BufferCore &tf,
00275                                           double range_cutoff = -1.0,
00276                                           int channel_options = channel_option::Default)
00277       {
00278         transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
00279       }
00280 
00281     protected:
00282 
00284 
00289       const boost::numeric::ublas::matrix<double>& getUnitVectors_(double angle_min,
00290                                                                    double angle_max,
00291                                                                    double angle_increment,
00292                                                                    unsigned int length);
00293 
00294     private:
00295 
00297       void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
00298                           sensor_msgs::PointCloud& cloud_out,
00299                           double range_cutoff,
00300                           bool preservative,
00301                           int channel_options);
00302 
00304       void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
00305                           sensor_msgs::PointCloud2 &cloud_out,
00306                           double range_cutoff,
00307                           int channel_options);
00308 
00310       void transformLaserScanToPointCloud_ (const std::string& target_frame,
00311                                             sensor_msgs::PointCloud& cloud_out,
00312                                             const sensor_msgs::LaserScan& scan_in,
00313                                             tf::Transformer & tf,
00314                                             double range_cutoff,
00315                                             int channel_options);
00316 
00318       void transformLaserScanToPointCloud_ (const std::string &target_frame,
00319                                             const sensor_msgs::LaserScan &scan_in,
00320                                             sensor_msgs::PointCloud2 &cloud_out,
00321                                             tf::Transformer &tf,
00322                                             double range_cutoff,
00323                                             int channel_options);
00324 
00326       void transformLaserScanToPointCloud_ (const std::string &target_frame,
00327                                             const sensor_msgs::LaserScan &scan_in,
00328                                             sensor_msgs::PointCloud2 &cloud_out,
00329                                             tf2::BufferCore &tf,
00330                                             double range_cutoff,
00331                                             int channel_options);
00332 
00334       void transformLaserScanToPointCloud_ (const std::string &target_frame,
00335                                             const sensor_msgs::LaserScan &scan_in,
00336                                             sensor_msgs::PointCloud2 &cloud_out,
00337                                             tf2::Quaternion quat_start,
00338                                             tf2::Vector3 origin_start,
00339                                             tf2::Quaternion quat_end,
00340                                             tf2::Vector3 origin_end,
00341                                             double range_cutoff,
00342                                             int channel_options);
00343 
00345       std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
00346       float angle_min_;
00347       float angle_max_;
00348       Eigen::ArrayXXd co_sine_map_;
00349       boost::mutex guv_mutex_;
00350     };
00351 
00352 }
00353 
00354 #endif //LASER_SCAN_UTILS_LASERSCAN_H


laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Mon Jul 24 2017 02:35:42