00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
00109
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
00148 void projectLaser (const sensor_msgs::LaserScan& scan_in,
00149 sensor_msgs::PointCloud2 &cloud_out,
00150 double range_cutoff = -1.0,
00151 int channel_options = channel_option::Default)
00152 {
00153 projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
00154 }
00155
00156
00158
00178 void transformLaserScanToPointCloud (const std::string& target_frame,
00179 const sensor_msgs::LaserScan& scan_in,
00180 sensor_msgs::PointCloud& cloud_out,
00181 tf::Transformer& tf,
00182 double range_cutoff,
00183 int channel_options = channel_option::Default)
00184 {
00185 return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options);
00186 }
00187
00189
00207 void transformLaserScanToPointCloud (const std::string& target_frame,
00208 const sensor_msgs::LaserScan& scan_in,
00209 sensor_msgs::PointCloud& cloud_out,
00210 tf::Transformer& tf,
00211 int channel_options = channel_option::Default)
00212 {
00213 return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options);
00214 }
00215
00217
00238 void transformLaserScanToPointCloud(const std::string &target_frame,
00239 const sensor_msgs::LaserScan &scan_in,
00240 sensor_msgs::PointCloud2 &cloud_out,
00241 tf::Transformer &tf,
00242 double range_cutoff = -1.0,
00243 int channel_options = channel_option::Default)
00244 {
00245 transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
00246 }
00247
00248 protected:
00249
00251
00256 const boost::numeric::ublas::matrix<double>& getUnitVectors_(double angle_min,
00257 double angle_max,
00258 double angle_increment,
00259 unsigned int length);
00260
00261 private:
00262
00264 void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
00265 sensor_msgs::PointCloud& cloud_out,
00266 double range_cutoff,
00267 bool preservative,
00268 int channel_options);
00269
00271 void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
00272 sensor_msgs::PointCloud2 &cloud_out,
00273 double range_cutoff,
00274 int channel_options);
00275
00277 void transformLaserScanToPointCloud_ (const std::string& target_frame,
00278 sensor_msgs::PointCloud& cloud_out,
00279 const sensor_msgs::LaserScan& scan_in,
00280 tf::Transformer & tf,
00281 double range_cutoff,
00282 int channel_options);
00283
00285 void transformLaserScanToPointCloud_ (const std::string &target_frame,
00286 const sensor_msgs::LaserScan &scan_in,
00287 sensor_msgs::PointCloud2 &cloud_out,
00288 tf::Transformer &tf,
00289 double range_cutoff,
00290 int channel_options);
00291
00293 std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
00294 float angle_min_;
00295 float angle_max_;
00296 Eigen::ArrayXXd co_sine_map_;
00297 boost::mutex guv_mutex_;
00298 };
00299
00300 }
00301
00302 #endif //LASER_SCAN_UTILS_LASERSCAN_H