laser_scan_assembler.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
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     // ***** Set Laser Projection Method *****
00056     private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true);
00057 
00058     // configure the filter chain from the parameter server
00059     filter_chain_.configure("filters", private_ns_);
00060 
00061     // Have different callbacks, depending on whether or not we want to ignore laser skews.
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     // apply filters on laser scan
00084     filter_chain_.update (scan_in, scan_filtered_);
00085 
00086     // convert laser scan to point cloud
00087     if (ignore_laser_skew_)  // Do it the fast (approximate) way
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                     // Do it the slower (more accurate) way
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_;   // The longest tolerance we've needed on a scan so far
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 }


laser_assembler
Author(s): Vijay Pradeep
autogenerated on Thu Apr 14 2016 11:56:55