$search
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 00042 #include "sensor_msgs/LaserScan.h" 00043 #include "sensor_msgs/PointCloud.h" 00044 #include "sensor_msgs/PointCloud.h" 00045 00046 #include <Eigen/Core> 00047 #include <sensor_msgs/PointCloud2.h> 00048 00049 namespace laser_geometry 00050 { 00051 // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle) 00052 const float LASER_SCAN_INVALID = -1.0; 00053 const float LASER_SCAN_MIN_RANGE = -2.0; 00054 const float LASER_SCAN_MAX_RANGE = -3.0; 00055 00056 namespace channel_option 00057 { 00059 00064 enum ChannelOption 00065 { 00066 None = 0x00, 00067 Intensity = 0x01, 00068 Index = 0x02, 00069 Distance = 0x04, 00070 Timestamp = 0x08, 00071 Viewpoint = 0x10, 00072 Default = (Intensity | Index) 00073 }; 00074 } 00075 00077 00098 class LaserProjection 00099 { 00100 00101 public: 00102 00103 LaserProjection() : angle_min_(0), angle_max_(0) {} 00104 00106 ~LaserProjection(); 00107 00108 00110 00124 void projectLaser (const sensor_msgs::LaserScan& scan_in, 00125 sensor_msgs::PointCloud& cloud_out, 00126 double range_cutoff = -1.0, 00127 int channel_options = channel_option::Default) 00128 { 00129 return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options); 00130 } 00131 00133 00147 void projectLaser (const sensor_msgs::LaserScan& scan_in, 00148 sensor_msgs::PointCloud2 &cloud_out, 00149 double range_cutoff = -1.0, 00150 int channel_options = channel_option::Default) 00151 { 00152 projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); 00153 } 00154 00155 00157 00177 void transformLaserScanToPointCloud (const std::string& target_frame, 00178 const sensor_msgs::LaserScan& scan_in, 00179 sensor_msgs::PointCloud& cloud_out, 00180 tf::Transformer& tf, 00181 double range_cutoff, 00182 int channel_options = channel_option::Default) 00183 { 00184 return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options); 00185 } 00186 00188 00206 void transformLaserScanToPointCloud (const std::string& target_frame, 00207 const sensor_msgs::LaserScan& scan_in, 00208 sensor_msgs::PointCloud& cloud_out, 00209 tf::Transformer& tf, 00210 int channel_options = channel_option::Default) 00211 { 00212 return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options); 00213 } 00214 00216 00236 void transformLaserScanToPointCloud(const std::string &target_frame, 00237 const sensor_msgs::LaserScan &scan_in, 00238 sensor_msgs::PointCloud2 &cloud_out, 00239 tf::Transformer &tf, 00240 double range_cutoff = -1.0, 00241 int channel_options = channel_option::Default) 00242 { 00243 transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); 00244 } 00245 00246 protected: 00247 00249 00254 const boost::numeric::ublas::matrix<double>& getUnitVectors_(double angle_min, 00255 double angle_max, 00256 double angle_increment, 00257 unsigned int length); 00258 00259 private: 00260 00262 void projectLaser_ (const sensor_msgs::LaserScan& scan_in, 00263 sensor_msgs::PointCloud& cloud_out, 00264 double range_cutoff, 00265 bool preservative, 00266 int channel_options); 00267 00269 void projectLaser_ (const sensor_msgs::LaserScan& scan_in, 00270 sensor_msgs::PointCloud2 &cloud_out, 00271 double range_cutoff, 00272 int channel_options); 00273 00275 void transformLaserScanToPointCloud_ (const std::string& target_frame, 00276 sensor_msgs::PointCloud& cloud_out, 00277 const sensor_msgs::LaserScan& scan_in, 00278 tf::Transformer & tf, 00279 double range_cutoff, 00280 int channel_options); 00281 00283 void transformLaserScanToPointCloud_ (const std::string &target_frame, 00284 const sensor_msgs::LaserScan &scan_in, 00285 sensor_msgs::PointCloud2 &cloud_out, 00286 tf::Transformer &tf, 00287 double range_cutoff, 00288 int channel_options); 00289 00291 std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_; 00292 float angle_min_; 00293 float angle_max_; 00294 Eigen::ArrayXXd co_sine_map_; 00295 boost::mutex guv_mutex_; 00296 }; 00297 00298 } 00299 00300 #endif //LASER_SCAN_UTILS_LASERSCAN_H