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   }
00120   else if (angle_min != scan.angle_min     ||
00121            angle_max != scan.angle_max     ||
00122            size      != scan.ranges.size())
00123   {
00124     throw std::runtime_error("laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask");
00125   }
00126 
00127   for (uint32_t i = 0; i < scan.ranges.size(); i++)
00128   {
00129     Sample* s = Sample::Extract(i, scan);
00130 
00131     if (s != NULL)
00132     {
00133       SampleSet::iterator m = mask_.find(s);
00134 
00135       if (m != mask_.end())
00136       {
00137         if ((*m)->range > s->range)
00138         {
00139           delete(*m);
00140           mask_.erase(m);
00141           mask_.insert(s);
00142         }
00143         else
00144         {
00145           delete s;
00146         }
00147       }
00148       else
00149       {
00150         mask_.insert(s);
00151       }
00152     }
00153   }
00154 }
00155 
00156 
00157 bool ScanMask::hasSample(Sample* s, float thresh)
00158 {
00159   if (s != NULL)
00160   {
00161     SampleSet::iterator m = mask_.find(s);
00162     if (m != mask_.end())
00163       if (((*m)->range - thresh) < s->range)
00164         return true;
00165   }
00166   return false;
00167 }
00168 
00169 
00170 
00171 ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold)
00172 {
00173   scan_ = scan;
00174 
00175   SampleSet* cluster = new SampleSet;
00176 
00177   for (uint32_t i = 0; i < scan.ranges.size(); i++)
00178   {
00179     Sample* s = Sample::Extract(i, scan);
00180 
00181     if (s != NULL)
00182     {
00183       if (!mask_.hasSample(s, mask_threshold))
00184       {
00185         cluster->insert(s);
00186       }
00187       else
00188       {
00189         delete s;
00190       }
00191     }
00192   }
00193 
00194   clusters_.push_back(cluster);
00195 
00196 }
00197 
00198 ScanProcessor::~ScanProcessor()
00199 {
00200   for (list<SampleSet*>::iterator c = clusters_.begin();
00201        c != clusters_.end();
00202        c++)
00203     delete(*c);
00204 }
00205 
00206 void
00207 ScanProcessor::removeLessThan(uint32_t num)
00208 {
00209   list<SampleSet*>::iterator c_iter = clusters_.begin();
00210   while (c_iter != clusters_.end())
00211   {
00212     if ((*c_iter)->size() < num)
00213     {
00214       delete(*c_iter);
00215       clusters_.erase(c_iter++);
00216     }
00217     else
00218     {
00219       ++c_iter;
00220     }
00221   }
00222 }
00223 
00224 
00225 void
00226 ScanProcessor::splitConnected(float thresh)
00227 {
00228   list<SampleSet*> tmp_clusters;
00229 
00230   list<SampleSet*>::iterator c_iter = clusters_.begin();
00231 
00232   // For each cluster
00233   while (c_iter != clusters_.end())
00234   {
00235     // Go through the entire list
00236     while ((*c_iter)->size() > 0)
00237     {
00238       // Take the first element
00239       SampleSet::iterator s_first = (*c_iter)->begin();
00240 
00241       // Start a new queue
00242       list<Sample*> sample_queue;
00243       sample_queue.push_back(*s_first);
00244 
00245       (*c_iter)->erase(s_first);
00246 
00247       // Grow until we get to the end of the queue
00248       list<Sample*>::iterator s_q = sample_queue.begin();
00249       while (s_q != sample_queue.end())
00250       {
00251         int expand = (int)(asin(thresh / (*s_q)->range) / std::abs(scan_.angle_increment));
00252 
00253         SampleSet::iterator s_rest = (*c_iter)->begin();
00254 
00255         while ((s_rest != (*c_iter)->end() &&
00256                 (*s_rest)->index < (*s_q)->index + expand))
00257         {
00258           if ((*s_rest)->range - (*s_q)->range > thresh)
00259           {
00260             break;
00261           }
00262           else if (sqrt(pow((*s_q)->x - (*s_rest)->x, 2.0f) + pow((*s_q)->y - (*s_rest)->y, 2.0f)) < thresh)
00263           {
00264             sample_queue.push_back(*s_rest);
00265             (*c_iter)->erase(s_rest++);
00266             break;
00267           }
00268           else
00269           {
00270             ++s_rest;
00271           }
00272         }
00273         s_q++;
00274       }
00275 
00276       // Move all the samples into the new cluster
00277       SampleSet* c = new SampleSet;
00278       for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
00279         c->insert(*s_q);
00280 
00281       // Store the temporary clusters
00282       tmp_clusters.push_back(c);
00283     }
00284 
00285     //Now that c_iter is empty, we can delete
00286     delete(*c_iter);
00287 
00288     //And remove from the map
00289     clusters_.erase(c_iter++);
00290   }
00291 
00292   clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end());
00293 }


leg_detector
Author(s): Caroline Pantofaru
autogenerated on Thu Apr 13 2017 02:41:52