laser_scan_assembler_srv.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_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     // ***** Set Laser Projection Method *****
00063     private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true);
00064 
00065     // configure the filter chain from the parameter server
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     // apply filters on laser scan
00082     filter_chain_.update (scan_in, scan_filtered_);
00083 
00084     // convert laser scan to point cloud
00085     if (ignore_laser_skew_)  // Do it the fast (approximate) way
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                     // Do it the slower (more accurate) way
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 }


laser_assembler
Author(s): Vijay Pradeep
autogenerated on Mon Oct 6 2014 01:43:03