costmap_to_polygons.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann, Otniel Rinaldo
00037  *********************************************************************/
00038 
00039 #include <costmap_converter/costmap_to_polygons.h>
00040 #include <boost/thread.hpp>
00041 #include <boost/thread/mutex.hpp>
00042 #include <pluginlib/class_list_macros.h>
00043 
00044 PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)
00045 
00046 namespace costmap_converter
00047 {
00048     
00049 CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()
00050 {
00051   costmap_ = NULL;
00052   dynamic_recfg_ = NULL;
00053 }
00054 
00055 CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH() 
00056 {
00057   if (dynamic_recfg_ != NULL)
00058     delete dynamic_recfg_;
00059 }
00060 
00061 void CostmapToPolygonsDBSMCCH::initialize(ros::NodeHandle nh)
00062 {
00063     costmap_ = NULL;
00064    
00065     max_distance_ = 0.4; 
00066     nh.param("cluster_max_distance", max_distance_, max_distance_);
00067     
00068     min_pts_ = 2;
00069     nh.param("cluster_min_pts", min_pts_, min_pts_);
00070     
00071     max_pts_ = 30;
00072     nh.param("cluster_max_pts", max_pts_, max_pts_);
00073     
00074     min_keypoint_separation_ = 0.1;
00075     nh.param("convex_hull_min_pt_separation", min_keypoint_separation_, min_keypoint_separation_);
00076     
00077     // setup dynamic reconfigure
00078     dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);
00079     dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);
00080     dynamic_recfg_->setCallback(cb);
00081 }
00082 
00083 
00084 void CostmapToPolygonsDBSMCCH::compute()
00085 {
00086     std::vector< std::vector<KeyPoint> > clusters;
00087     dbScan(occupied_cells_, clusters);
00088     
00089     // Create new polygon container
00090     PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());
00091     
00092     
00093     // add convex hulls to polygon container
00094     for (std::size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
00095     {
00096       polygons->push_back( geometry_msgs::Polygon() );
00097       convexHull2(clusters[i], polygons->back() );
00098     }
00099     
00100     // add our non-cluster points to the polygon container (as single points)
00101     if (!clusters.empty())
00102     {
00103       for (std::size_t i=0; i < clusters.front().size(); ++i)
00104       {
00105         polygons->push_back( geometry_msgs::Polygon() );
00106         convertPointToPolygon(clusters.front()[i], polygons->back());
00107       }
00108     }
00109     
00110     // replace shared polygon container
00111     updatePolygonContainer(polygons);
00112 }
00113 
00114 void CostmapToPolygonsDBSMCCH::setCostmap2D(costmap_2d::Costmap2D *costmap)
00115 {
00116     if (!costmap)
00117       return;
00118 
00119     costmap_ = costmap;     
00120     
00121     updateCostmap2D();
00122 }
00123 
00124 void CostmapToPolygonsDBSMCCH::updateCostmap2D()
00125 {
00126       occupied_cells_.clear();
00127       
00128       if (!costmap_->getMutex())
00129       {
00130         ROS_ERROR("Cannot update costmap since the mutex pointer is null");
00131         return;
00132       }
00133       
00134       int idx = 0;
00135       
00136       costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
00137             
00138       // get indices of obstacle cells
00139       for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
00140       {
00141         for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
00142         {
00143           int value = costmap_->getCost(i,j);
00144           if(value >= costmap_2d::LETHAL_OBSTACLE)
00145           {
00146             double x, y;
00147             costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
00148             occupied_cells_.push_back( KeyPoint( x, y ) );
00149           }
00150           ++idx;
00151         }
00152       }
00153 }
00154 
00155 
00156 void CostmapToPolygonsDBSMCCH::dbScan(const std::vector<KeyPoint>& occupied_cells, std::vector< std::vector<KeyPoint> >& clusters)
00157 {
00158   std::vector<bool> visited(occupied_cells.size(), false);
00159 
00160   clusters.clear();  
00161   
00162   //DB Scan Algorithm
00163   int cluster_id = 0; // current cluster_id
00164   clusters.push_back(std::vector<KeyPoint>());
00165   for(int i = 0; i< (int)occupied_cells.size(); i++)
00166   {
00167     if(!visited[i]) //keypoint has not been visited before
00168     {
00169       visited[i] = true; // mark as visited
00170       std::vector<int> neighbors;
00171       regionQuery(occupied_cells, i, neighbors); //Find neighbors around the keypoint
00172       if((int)neighbors.size() < min_pts_) //If not enough neighbors are found, mark as noise
00173       {         
00174         clusters[0].push_back(occupied_cells[i]);
00175       }
00176       else
00177       {
00178         ++cluster_id; // increment current cluster_id
00179         clusters.push_back(std::vector<KeyPoint>());
00180         
00181         // Expand the cluster
00182         clusters[cluster_id].push_back(occupied_cells[i]);
00183         for(int j = 0; j<(int)neighbors.size(); j++)
00184         {
00185           if ((int)clusters[cluster_id].size() == max_pts_)
00186             break;
00187           
00188           if(!visited[neighbors[j]]) //keypoint has not been visited before
00189           {
00190             visited[neighbors[j]] = true;  // mark as visited
00191             std::vector<int> further_neighbors;
00192             regionQuery(occupied_cells, neighbors[j], further_neighbors); //Find more neighbors around the new keypoint
00193 //             if(further_neighbors.size() < min_pts_)
00194 //             {          
00195 //               clusters[0].push_back(occupied_cells[neighbors[j]]);
00196 //             }
00197 //             else
00198             if ((int)further_neighbors.size() >= min_pts_)
00199             {
00200               // neighbors found
00201               neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end());  //Add these newfound P' neighbour to P neighbour vector "nb_indeces"
00202               clusters[cluster_id].push_back(occupied_cells[neighbors[j]]);
00203             }
00204           }
00205         }
00206       }       
00207     }
00208   } 
00209 }
00210   
00211 void CostmapToPolygonsDBSMCCH::regionQuery(const std::vector<KeyPoint>& occupied_cells, int curr_index, std::vector<int>& neighbors)
00212 {
00213     neighbors.clear();
00214     double curr_index_x = occupied_cells[curr_index].x;
00215     double curr_index_y = occupied_cells[curr_index].y;
00216 
00217     for(int i = 0; i < (int)occupied_cells.size(); i++)
00218     {
00219       double neighbor_x = occupied_cells[i].x;
00220       double neighbor_y = occupied_cells[i].y;
00221       double dist = sqrt(pow((curr_index_x - neighbor_x),2)+pow((curr_index_y - neighbor_y),2));        //euclidean distance between two points // TODO map resolution
00222       if(dist <= max_distance_ && dist != 0.0f)
00223         neighbors.push_back(i);
00224     }
00225 }
00226 
00227 bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2)
00228 {
00229   return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
00230 }
00231 
00232 void CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)
00233 {
00234     //Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull
00235   
00236     int k = 0;
00237     int n = cluster.size();
00238     
00239     // sort points according to x coordinate (TODO. is it already sorted due to the map representation?)
00240     std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);
00241     
00242     polygon.points.resize(2*n);
00243       
00244     // lower hull
00245     for (int i = 0; i < n; ++i)
00246     {
00247       while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0) 
00248       {
00249         --k;
00250       }
00251       cluster[i].toPointMsg(polygon.points[k]);
00252       ++k;
00253     }
00254       
00255     // upper hull  
00256     for (int i = n-2, t = k+1; i >= 0; --i) 
00257     {
00258       while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
00259       {
00260         --k;
00261       }
00262       cluster[i].toPointMsg(polygon.points[k]);
00263       ++k;
00264     }
00265     
00266 
00267     polygon.points.resize(k); // original
00268     // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.
00269 //     polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost
00270 
00271     
00272     
00273     if (min_keypoint_separation_>0) // TODO maybe migrate to algorithm above to speed up computation
00274     {
00275       for (int i=0; i < (int) polygon.points.size() - 1; ++i)
00276       {
00277         if ( std::sqrt(std::pow((polygon.points[i].x - polygon.points[i+1].x),2) + std::pow((polygon.points[i].y - polygon.points[i+1].y),2)) < min_keypoint_separation_ )
00278           polygon.points.erase(polygon.points.begin()+i+1);
00279       }
00280     }
00281 }
00282 
00283 
00284 
00285 void CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)
00286 {
00287     std::vector<KeyPoint> P = cluster;
00288     std::vector<geometry_msgs::Point32>& points = polygon.points;
00289 
00290     // Sort P by x and y
00291     for (int i = 0; i < (int)P.size(); i++)
00292     {
00293         for (int j = i + 1; j < (int)P.size(); j++) 
00294         {
00295             if (P[j].x < P[i].x || (P[j].x == P[i].x && P[j].y < P[i].y))
00296             {
00297                 KeyPoint tmp = P[i];
00298                 P[i] = P[j];
00299                 P[j] = tmp;
00300             }
00301         }
00302     }
00303 
00304     // the output array H[] will be used as the stack
00305     int i;                 // array scan index
00306 
00307     // Get the indices of points with min x-coord and min|max y-coord
00308     int minmin = 0, minmax;
00309     double xmin = P[0].x;
00310     for (i = 1; i < (int)P.size(); i++)
00311         if (P[i].x != xmin) break;
00312     minmax = i - 1;
00313     if (minmax == (int)P.size() - 1)
00314     {   // degenerate case: all x-coords == xmin
00315         points.push_back(geometry_msgs::Point32());
00316         P[minmin].toPointMsg(points.back());
00317         if (P[minmax].y != P[minmin].y) // a  nontrivial segment
00318         {
00319             points.push_back(geometry_msgs::Point32());
00320             P[minmax].toPointMsg(points.back());
00321         }
00322         // add polygon endpoint
00323         points.push_back(geometry_msgs::Point32());
00324         P[minmin].toPointMsg(points.back());
00325         return;
00326     }
00327 
00328     // Get the indices of points with max x-coord and min|max y-coord
00329     int maxmin, maxmax = (int)P.size() - 1;
00330     double xmax = P.back().x;
00331     for (i = P.size() - 2; i >= 0; i--)
00332         if (P[i].x != xmax) break;
00333     maxmin = i+1;
00334 
00335     // Compute the lower hull on the stack H
00336     // push  minmin point onto stack
00337     points.push_back(geometry_msgs::Point32());
00338     P[minmin].toPointMsg(points.back());
00339     i = minmax;
00340     while (++i <= maxmin)
00341     {
00342         // the lower line joins P[minmin]  with P[maxmin]
00343         if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
00344             continue;           // ignore P[i] above or on the lower line
00345 
00346         while (points.size() > 1)         // there are at least 2 points on the stack
00347         {
00348             // test if  P[i] is left of the line at the stack top
00349             if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
00350                 break;         // P[i] is a new hull  vertex
00351             points.pop_back();         // pop top point off  stack
00352         }
00353         // push P[i] onto stack
00354         points.push_back(geometry_msgs::Point32());
00355         P[i].toPointMsg(points.back());
00356     }
00357 
00358     // Next, compute the upper hull on the stack H above  the bottom hull
00359     if (maxmax != maxmin)      // if  distinct xmax points
00360     {
00361          // push maxmax point onto stack
00362          points.push_back(geometry_msgs::Point32());
00363          P[maxmax].toPointMsg(points.back());
00364     }
00365     int bot = (int)points.size();                  // the bottom point of the upper hull stack
00366     i = maxmin;
00367     while (--i >= minmax)
00368     {
00369         // the upper line joins P[maxmax]  with P[minmax]
00370         if (cross( P[maxmax], P[minmax], P[i])  >= 0 && i > minmax)
00371             continue;           // ignore P[i] below or on the upper line
00372 
00373         while ((int)points.size() > bot)     // at least 2 points on the upper stack
00374         {
00375             // test if  P[i] is left of the line at the stack top
00376             if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
00377                 break;         // P[i] is a new hull  vertex
00378             points.pop_back();         // pop top point off stack
00379         }
00380         // push P[i] onto stack
00381         points.push_back(geometry_msgs::Point32());
00382         P[i].toPointMsg(points.back());
00383     }
00384     if (minmax != minmin)
00385     {
00386         // push  joining endpoint onto stack
00387         points.push_back(geometry_msgs::Point32());
00388         P[minmin].toPointMsg(points.back());
00389     }
00390     
00391     if (min_keypoint_separation_>0) // TODO maybe migrate to algorithm above to speed up computation
00392     {
00393       for (int i=0; i < (int) polygon.points.size() - 1; ++i)
00394       {
00395         if ( std::sqrt(std::pow((polygon.points[i].x - polygon.points[i+1].x),2) + std::pow((polygon.points[i].y - polygon.points[i+1].y),2)) < min_keypoint_separation_ )
00396           polygon.points.erase(polygon.points.begin()+i+1);
00397       }
00398     }
00399 }
00400 
00401 void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)
00402 {
00403   boost::mutex::scoped_lock lock(mutex_);
00404   polygons_ = polygons;
00405 }
00406 
00407 
00408 PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
00409 {
00410   boost::mutex::scoped_lock lock(mutex_);
00411   PolygonContainerConstPtr polygons = polygons_;
00412   return polygons;
00413 }
00414 
00415 void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level)
00416 {
00417     max_distance_ = config.cluster_max_distance;
00418     min_pts_ = config.cluster_min_pts;
00419     max_pts_ = config.cluster_max_pts;
00420     min_keypoint_separation_ = config.cluster_min_pts;
00421 }
00422 
00423 }//end namespace costmap_converter
00424 
00425 


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat Jun 8 2019 20:53:15