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_srv.h"
00039 #include "filters/filter_chain.h"
00040
00041 using namespace laser_geometry;
00042 using namespace std ;
00043
00044 namespace laser_assembler
00045 {
00046
00057 class LaserScanAssemblerSrv : public BaseAssemblerSrv<sensor_msgs::LaserScan>
00058 {
00059 public:
00060 LaserScanAssemblerSrv() : filter_chain_("sensor_msgs::LaserScan")
00061 {
00062
00063 private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true);
00064
00065
00066 filter_chain_.configure("filters", private_ns_);
00067 }
00068
00069 ~LaserScanAssemblerSrv()
00070 {
00071
00072 }
00073
00074 unsigned int GetPointsInScan(const sensor_msgs::LaserScan& scan)
00075 {
00076 return scan.ranges.size();
00077 }
00078
00079 void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out)
00080 {
00081
00082 filter_chain_.update (scan_in, scan_filtered_);
00083
00084
00085 if (ignore_laser_skew_)
00086 {
00087 projector_.projectLaser(scan_filtered_, cloud_out);
00088 if (cloud_out.header.frame_id != fixed_frame_id)
00089 tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out);
00090 }
00091 else
00092 {
00093 int mask = laser_geometry::channel_option::Intensity + laser_geometry::channel_option::Distance + laser_geometry::channel_option::Index + laser_geometry::channel_option::Timestamp;
00094 projector_.transformLaserScanToPointCloud (fixed_frame_id, scan_filtered_, cloud_out, *tf_, mask);
00095 }
00096 return;
00097 }
00098
00099 private:
00100 bool ignore_laser_skew_;
00101 laser_geometry::LaserProjection projector_;
00102
00103 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
00104 mutable sensor_msgs::LaserScan scan_filtered_;
00105
00106 };
00107
00108 }
00109
00110 using namespace laser_assembler ;
00111
00112 int main(int argc, char **argv)
00113 {
00114 ros::init(argc, argv, "laser_scan_assembler");
00115 ros::NodeHandle n;
00116 ROS_WARN("The laser_scan_assembler_srv is deprecated. Please switch to "
00117 "using the laser_scan_assembler. Documentation is available at "
00118 "http://www.ros.org/wiki/laser_assembler");
00119 LaserScanAssemblerSrv pc_assembler;
00120 pc_assembler.start();
00121 ros::spin();
00122
00123 return 0;
00124 }