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 #include <ros/macros.h>
00051
00052
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
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