laser_processor.h
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 
00039 
00040 
00041 
00042 #ifndef LASER_SCAN_LASERPROCESSOR_HH
00043 #define LASER_SCAN_LASERPROCESSOR_HH
00044 
00045 #include <unistd.h>
00046 #include <math.h>
00047 #include "sensor_msgs/LaserScan.h"
00048 #include "sensor_msgs/PointCloud.h"
00049 #include "geometry_msgs/Point.h"
00050 
00051 #include <list>
00052 #include <set>
00053 #include <vector>
00054 #include <map>
00055 #include <utility>
00056 #include <algorithm>
00057 
00058 #include "tf/transform_datatypes.h"
00059 
00060 namespace laser_processor
00061 {
00063   class Sample
00064   {
00065   public:
00066     int   index;
00067     float range;
00068     float intensity;
00069     float x;
00070     float y;
00071 
00072     static Sample* Extract(int ind, const sensor_msgs::LaserScan& scan);
00073 
00074   private:
00075      Sample() {};
00076   };
00077 
00079   struct CompareSample
00080   {
00081      CompareSample() {}
00082 
00083     inline bool operator() (const Sample* a, const Sample* b)
00084     {
00085       return (a->index <  b->index);
00086     }
00087   };
00088 
00089 
00091   class SampleSet : public std::set<Sample*, CompareSample>
00092   {
00093   public:
00094     SampleSet() {}
00095 
00096     ~SampleSet() { clear(); }
00097 
00098     void clear();
00099 
00100     void appendToCloud(sensor_msgs::PointCloud& cloud, int r = 0, int g = 0, int b = 0);
00101 
00102     tf::Point center();
00103   };
00104 
00106   class ScanMask
00107   {
00108     SampleSet mask_;
00109 
00110     bool     filled;
00111     float    angle_min;
00112     float    angle_max;
00113     uint32_t size;
00114 
00115   public:
00116 
00117     ScanMask() : filled(false), angle_min(0), angle_max(0), size(0) { }
00118 
00119     inline void clear() { mask_.clear(); filled = false; }
00120 
00121     void addScan(sensor_msgs::LaserScan& scan);
00122 
00123     bool hasSample(Sample* s, float thresh);
00124   };
00125 
00126 
00127 
00128   class ScanProcessor
00129   {
00130     std::list<SampleSet*> clusters_;
00131     sensor_msgs::LaserScan scan_;
00132 
00133   public:
00134 
00135     std::list<SampleSet*>& getClusters() { return clusters_; }
00136 
00137     ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold = 0.03);
00138 
00139     ~ScanProcessor();
00140 
00141     void removeLessThan(uint32_t num);
00142 
00143     void splitConnected(float thresh);
00144   };
00145 };
00146 
00147 #endif


leg_detector
Author(s): Caroline Pantofaru
autogenerated on Thu Aug 27 2015 14:18:07