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()
00097   {
00098     clear();
00099   }
00100 
00101   void clear();
00102 
00103   void appendToCloud(sensor_msgs::PointCloud& cloud, int r = 0, int g = 0, int b = 0);
00104 
00105   tf::Point center();
00106 };
00107 
00109 class ScanMask
00110 {
00111   SampleSet mask_;
00112 
00113   bool     filled;
00114   float    angle_min;
00115   float    angle_max;
00116   uint32_t size;
00117 
00118 public:
00119 
00120   ScanMask() : filled(false), angle_min(0), angle_max(0), size(0) { }
00121 
00122   inline void clear()
00123   {
00124     mask_.clear();
00125     filled = false;
00126   }
00127 
00128   void addScan(sensor_msgs::LaserScan& scan);
00129 
00130   bool hasSample(Sample* s, float thresh);
00131 };
00132 
00133 
00134 
00135 class ScanProcessor
00136 {
00137   std::list<SampleSet*> clusters_;
00138   sensor_msgs::LaserScan scan_;
00139 
00140 public:
00141 
00142   std::list<SampleSet*>& getClusters()
00143   {
00144     return clusters_;
00145   }
00146 
00147   ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold = 0.03);
00148 
00149   ~ScanProcessor();
00150 
00151   void removeLessThan(uint32_t num);
00152 
00153   void splitConnected(float thresh);
00154 };
00155 };
00156 
00157 #endif


leg_detector
Author(s): Caroline Pantofaru
autogenerated on Sat Jun 8 2019 18:40:29