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 #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
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