Go to the documentation of this file.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
00031
00032
00033
00034
00035
00036 #include "laser_geometry/laser_geometry.h"
00037 #include "sensor_msgs/LaserScan.h"
00038 #include "laser_assembler/base_assembler.h"
00039 #include "filters/filter_chain.h"
00040
00041 using namespace laser_geometry;
00042 using namespace std ;
00043
00044 namespace laser_assembler
00045 {
00046
00050 class LaserScanAssembler : public BaseAssembler<sensor_msgs::LaserScan>
00051 {
00052 public:
00053 LaserScanAssembler() : BaseAssembler<sensor_msgs::LaserScan>("max_scans"), filter_chain_("sensor_msgs::LaserScan")
00054 {
00055
00056 private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true);
00057
00058
00059 filter_chain_.configure("filters", private_ns_);
00060
00061
00062 if (ignore_laser_skew_)
00063 start("scan");
00064 else
00065 {
00066 start();
00067 skew_scan_sub_ = n_.subscribe("scan", 10, &LaserScanAssembler::scanCallback, this);
00068 }
00069 }
00070
00071 ~LaserScanAssembler()
00072 {
00073
00074 }
00075
00076 unsigned int GetPointsInScan(const sensor_msgs::LaserScan& scan)
00077 {
00078 return (scan.ranges.size ());
00079 }
00080
00081 void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out)
00082 {
00083
00084 filter_chain_.update (scan_in, scan_filtered_);
00085
00086
00087 if (ignore_laser_skew_)
00088 {
00089 projector_.projectLaser(scan_filtered_, cloud_out);
00090 if (cloud_out.header.frame_id != fixed_frame_id)
00091 tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out);
00092 }
00093 else
00094 {
00095 int mask = laser_geometry::channel_option::Intensity +
00096 laser_geometry::channel_option::Distance +
00097 laser_geometry::channel_option::Index +
00098 laser_geometry::channel_option::Timestamp;
00099 projector_.transformLaserScanToPointCloud (fixed_frame_id, scan_filtered_, cloud_out, *tf_, mask);
00100 }
00101 return;
00102 }
00103
00104 void scanCallback(const sensor_msgs::LaserScanConstPtr& laser_scan)
00105 {
00106 if (!ignore_laser_skew_)
00107 {
00108 ros::Duration cur_tolerance = ros::Duration(laser_scan->time_increment * laser_scan->ranges.size());
00109 if (cur_tolerance > max_tolerance_)
00110 {
00111 ROS_DEBUG("Upping tf tolerance from [%.4fs] to [%.4fs]", max_tolerance_.toSec(), cur_tolerance.toSec());
00112 assert(tf_filter_);
00113 tf_filter_->setTolerance(cur_tolerance);
00114 max_tolerance_ = cur_tolerance;
00115 }
00116 tf_filter_->add(laser_scan);
00117 }
00118 }
00119
00120 private:
00121 bool ignore_laser_skew_;
00122 laser_geometry::LaserProjection projector_;
00123
00124 ros::Subscriber skew_scan_sub_;
00125 ros::Duration max_tolerance_;
00126
00127 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
00128 mutable sensor_msgs::LaserScan scan_filtered_;
00129
00130 };
00131
00132 }
00133
00134 using namespace laser_assembler ;
00135
00136 int main(int argc, char **argv)
00137 {
00138 ros::init(argc, argv, "laser_scan_assembler");
00139 LaserScanAssembler pc_assembler;
00140 ros::spin();
00141
00142 return 0;
00143 }