laser_processor.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 #include <leg_detector/laser_processor.h>
00036 
00037 #include <stdexcept>
00038 
00039 using namespace ros;
00040 using namespace std;
00041 using namespace laser_processor;
00042 
00043 Sample* Sample::Extract(int ind, const sensor_msgs::LaserScan& scan)
00044 {
00045   Sample* s = new Sample();
00046 
00047   s->index = ind;
00048   s->range = scan.ranges[ind];
00049   s->x = cos( scan.angle_min + ind*scan.angle_increment ) * s->range;
00050   s->y = sin( scan.angle_min + ind*scan.angle_increment ) * s->range;
00051   if (s->range > scan.range_min && s->range < scan.range_max)
00052     return s;
00053   else
00054   {
00055     delete s;
00056     return NULL;
00057   }
00058 }
00059 
00060 void SampleSet::clear()
00061 {
00062   for (SampleSet::iterator i = begin();
00063        i != end();
00064        i++)
00065   {
00066     delete (*i);
00067   }
00068   set<Sample*, CompareSample>::clear();
00069 }
00070 
00071 void SampleSet::appendToCloud(sensor_msgs::PointCloud& cloud, int r, int g, int b)
00072 {
00073   float color_val = 0;
00074 
00075   int rgb = (r << 16) | (g << 8) | b;
00076   color_val = *(float*)&(rgb);
00077 
00078   for (iterator sample_iter = begin();
00079        sample_iter != end();
00080        sample_iter++)
00081   {
00082     geometry_msgs::Point32 point;
00083     point.x = (*sample_iter)->x;
00084     point.y = (*sample_iter)->y;
00085     point.z = 0;
00086 
00087     cloud.points.push_back(point);
00088 
00089     if (cloud.channels[0].name == "rgb")
00090       cloud.channels[0].values.push_back(color_val);
00091   }
00092 }
00093 
00094 tf::Point SampleSet::center()
00095 {
00096   float x_mean = 0.0;
00097   float y_mean = 0.0;
00098   for (iterator i = begin();
00099        i != end();
00100        i++)
00101 
00102   {
00103     x_mean += ((*i)->x)/size();
00104     y_mean += ((*i)->y)/size();
00105   }
00106 
00107   return tf::Point (x_mean, y_mean, 0.0);
00108 }
00109 
00110 
00111 void ScanMask::addScan(sensor_msgs::LaserScan& scan)
00112 {
00113   if (!filled)
00114   {
00115     angle_min = scan.angle_min;
00116     angle_max = scan.angle_max;
00117     size      = scan.ranges.size();
00118     filled    = true;
00119   } else if (angle_min != scan.angle_min     ||
00120              angle_max != scan.angle_max     ||
00121              size      != scan.ranges.size())
00122   {
00123     throw std::runtime_error("laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask");
00124   }
00125 
00126   for (uint32_t i = 0; i < scan.ranges.size(); i++)
00127   {
00128     Sample* s = Sample::Extract(i, scan);
00129 
00130     if (s != NULL)
00131     {
00132       SampleSet::iterator m = mask_.find(s);
00133 
00134       if (m != mask_.end())
00135       {
00136         if ((*m)->range > s->range)
00137         {
00138           delete (*m);
00139           mask_.erase(m);
00140           mask_.insert(s);
00141         } else {
00142           delete s;
00143         }
00144       }
00145       else
00146       {
00147         mask_.insert(s);
00148       }
00149     }
00150   }
00151 }
00152 
00153 
00154 bool ScanMask::hasSample(Sample* s, float thresh)
00155 {
00156   if (s != NULL)
00157   {
00158     SampleSet::iterator m = mask_.find(s);
00159     if ( m != mask_.end())
00160       if (((*m)->range - thresh) < s->range)
00161         return true;
00162   }
00163   return false;
00164 }
00165 
00166 
00167 
00168 ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold)
00169 {
00170   scan_ = scan;
00171 
00172   SampleSet* cluster = new SampleSet;
00173 
00174   for (uint32_t i = 0; i < scan.ranges.size(); i++)
00175   {
00176     Sample* s = Sample::Extract(i, scan);
00177 
00178     if (s != NULL)
00179     {
00180       if (!mask_.hasSample(s, mask_threshold))
00181       {
00182         cluster->insert(s);
00183       } else {
00184         delete s;
00185       }
00186     }
00187   }
00188 
00189   clusters_.push_back(cluster);
00190 
00191 }
00192 
00193 ScanProcessor::~ScanProcessor()
00194 {
00195   for ( list<SampleSet*>::iterator c = clusters_.begin();
00196         c != clusters_.end();
00197         c++)
00198     delete (*c);
00199 }
00200 
00201 void
00202 ScanProcessor::removeLessThan(uint32_t num)
00203 {
00204   list<SampleSet*>::iterator c_iter = clusters_.begin();
00205   while (c_iter != clusters_.end())
00206   {
00207     if ( (*c_iter)->size() < num )
00208     {
00209       delete (*c_iter);
00210       clusters_.erase(c_iter++);
00211     } else {
00212       ++c_iter;
00213     }
00214   }
00215 }
00216 
00217 
00218 void
00219 ScanProcessor::splitConnected(float thresh)
00220 {
00221   list<SampleSet*> tmp_clusters;
00222 
00223   list<SampleSet*>::iterator c_iter = clusters_.begin();
00224 
00225   // For each cluster
00226   while (c_iter != clusters_.end())
00227   {
00228     // Go through the entire list
00229     while ((*c_iter)->size() > 0 )
00230     {
00231       // Take the first element
00232       SampleSet::iterator s_first = (*c_iter)->begin();
00233 
00234       // Start a new queue
00235       list<Sample*> sample_queue;
00236       sample_queue.push_back(*s_first);
00237 
00238       (*c_iter)->erase(s_first);
00239 
00240       // Grow until we get to the end of the queue
00241       list<Sample*>::iterator s_q = sample_queue.begin();
00242       while (s_q != sample_queue.end())
00243       {
00244         int expand = (int)(asin( thresh / (*s_q)->range ) / scan_.angle_increment);
00245 
00246         SampleSet::iterator s_rest = (*c_iter)->begin();
00247 
00248         while ( (s_rest != (*c_iter)->end() &&
00249                  (*s_rest)->index < (*s_q)->index + expand ) )
00250         {
00251           if ( (*s_rest)->range - (*s_q)->range > thresh)
00252           {
00253             break;
00254           }
00255           else if (sqrt( pow( (*s_q)->x - (*s_rest)->x, 2.0f) + pow( (*s_q)->y - (*s_rest)->y, 2.0f)) < thresh)
00256           {
00257             sample_queue.push_back(*s_rest);
00258             (*c_iter)->erase(s_rest++);
00259             break;
00260           } else {
00261             ++s_rest;
00262           }
00263         }
00264         s_q++;
00265       }
00266 
00267       // Move all the samples into the new cluster
00268       SampleSet* c = new SampleSet;
00269       for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
00270         c->insert(*s_q);
00271 
00272       // Store the temporary clusters
00273       tmp_clusters.push_back(c);
00274     }
00275 
00276     //Now that c_iter is empty, we can delete
00277     delete (*c_iter);
00278 
00279     //And remove from the map
00280     clusters_.erase(c_iter++);
00281   }
00282 
00283   clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end());
00284 }


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