footprint_helper.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: TKruse
00036  *********************************************************************/
00037 
00038 #include <base_local_planner/footprint_helper.h>
00039 
00040 namespace base_local_planner {
00041 
00042 FootprintHelper::FootprintHelper() {
00043   // TODO Auto-generated constructor stub
00044 
00045 }
00046 
00047 FootprintHelper::~FootprintHelper() {
00048   // TODO Auto-generated destructor stub
00049 }
00050 
00051 void FootprintHelper::getLineCells(int x0, int x1, int y0, int y1, std::vector<base_local_planner::Position2DInt>& pts) {
00052   //Bresenham Ray-Tracing
00053   int deltax = abs(x1 - x0);        // The difference between the x's
00054   int deltay = abs(y1 - y0);        // The difference between the y's
00055   int x = x0;                       // Start x off at the first pixel
00056   int y = y0;                       // Start y off at the first pixel
00057 
00058   int xinc1, xinc2, yinc1, yinc2;
00059   int den, num, numadd, numpixels;
00060 
00061   base_local_planner::Position2DInt pt;
00062 
00063   if (x1 >= x0)                 // The x-values are increasing
00064   {
00065     xinc1 = 1;
00066     xinc2 = 1;
00067   }
00068   else                          // The x-values are decreasing
00069   {
00070     xinc1 = -1;
00071     xinc2 = -1;
00072   }
00073 
00074   if (y1 >= y0)                 // The y-values are increasing
00075   {
00076     yinc1 = 1;
00077     yinc2 = 1;
00078   }
00079   else                          // The y-values are decreasing
00080   {
00081     yinc1 = -1;
00082     yinc2 = -1;
00083   }
00084 
00085   if (deltax >= deltay)         // There is at least one x-value for every y-value
00086   {
00087     xinc1 = 0;                  // Don't change the x when numerator >= denominator
00088     yinc2 = 0;                  // Don't change the y for every iteration
00089     den = deltax;
00090     num = deltax / 2;
00091     numadd = deltay;
00092     numpixels = deltax;         // There are more x-values than y-values
00093   }
00094   else                          // There is at least one y-value for every x-value
00095   {
00096     xinc2 = 0;                  // Don't change the x for every iteration
00097     yinc1 = 0;                  // Don't change the y when numerator >= denominator
00098     den = deltay;
00099     num = deltay / 2;
00100     numadd = deltax;
00101     numpixels = deltay;         // There are more y-values than x-values
00102   }
00103 
00104   for (int curpixel = 0; curpixel <= numpixels; curpixel++)
00105   {
00106     pt.x = x;      //Draw the current pixel
00107     pt.y = y;
00108     pts.push_back(pt);
00109 
00110     num += numadd;              // Increase the numerator by the top of the fraction
00111     if (num >= den)             // Check if numerator >= denominator
00112     {
00113       num -= den;               // Calculate the new numerator value
00114       x += xinc1;               // Change the x as appropriate
00115       y += yinc1;               // Change the y as appropriate
00116     }
00117     x += xinc2;                 // Change the x as appropriate
00118     y += yinc2;                 // Change the y as appropriate
00119   }
00120 }
00121 
00122 
00123 void FootprintHelper::getFillCells(std::vector<base_local_planner::Position2DInt>& footprint){
00124   //quick bubble sort to sort pts by x
00125   base_local_planner::Position2DInt swap, pt;
00126   unsigned int i = 0;
00127   while (i < footprint.size() - 1) {
00128     if (footprint[i].x > footprint[i + 1].x) {
00129       swap = footprint[i];
00130       footprint[i] = footprint[i + 1];
00131       footprint[i + 1] = swap;
00132       if(i > 0) {
00133         --i;
00134       }
00135     } else {
00136       ++i;
00137     }
00138   }
00139 
00140   i = 0;
00141   base_local_planner::Position2DInt min_pt;
00142   base_local_planner::Position2DInt max_pt;
00143   unsigned int min_x = footprint[0].x;
00144   unsigned int max_x = footprint[footprint.size() -1].x;
00145   //walk through each column and mark cells inside the footprint
00146   for (unsigned int x = min_x; x <= max_x; ++x) {
00147     if (i >= footprint.size() - 1) {
00148       break;
00149     }
00150     if (footprint[i].y < footprint[i + 1].y) {
00151       min_pt = footprint[i];
00152       max_pt = footprint[i + 1];
00153     } else {
00154       min_pt = footprint[i + 1];
00155       max_pt = footprint[i];
00156     }
00157 
00158     i += 2;
00159     while (i < footprint.size() && footprint[i].x == x) {
00160       if(footprint[i].y < min_pt.y) {
00161         min_pt = footprint[i];
00162       } else if(footprint[i].y > max_pt.y) {
00163         max_pt = footprint[i];
00164       }
00165       ++i;
00166     }
00167 
00168     //loop though cells in the column
00169     for (unsigned int y = min_pt.y; y < max_pt.y; ++y) {
00170       pt.x = x;
00171       pt.y = y;
00172       footprint.push_back(pt);
00173     }
00174   }
00175 }
00176 
00180 std::vector<base_local_planner::Position2DInt> FootprintHelper::getFootprintCells(
00181     Eigen::Vector3f pos,
00182     std::vector<geometry_msgs::Point> footprint_spec,
00183     const costmap_2d::Costmap2D& costmap,
00184     bool fill){
00185   double x_i = pos[0];
00186   double y_i = pos[1];
00187   double theta_i = pos[2];
00188   std::vector<base_local_planner::Position2DInt> footprint_cells;
00189 
00190   //if we have no footprint... do nothing
00191   if (footprint_spec.size() <= 1) {
00192     unsigned int mx, my;
00193     if (costmap.worldToMap(x_i, y_i, mx, my)) {
00194       Position2DInt center;
00195       center.x = mx;
00196       center.y = my;
00197       footprint_cells.push_back(center);
00198     }
00199     return footprint_cells;
00200   }
00201 
00202   //pre-compute cos and sin values
00203   double cos_th = cos(theta_i);
00204   double sin_th = sin(theta_i);
00205   double new_x, new_y;
00206   unsigned int x0, y0, x1, y1;
00207   unsigned int last_index = footprint_spec.size() - 1;
00208 
00209   for (unsigned int i = 0; i < last_index; ++i) {
00210     //find the cell coordinates of the first segment point
00211     new_x = x_i + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
00212     new_y = y_i + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
00213     if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
00214       return footprint_cells;
00215     }
00216 
00217     //find the cell coordinates of the second segment point
00218     new_x = x_i + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
00219     new_y = y_i + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
00220     if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
00221       return footprint_cells;
00222     }
00223 
00224     getLineCells(x0, x1, y0, y1, footprint_cells);
00225   }
00226 
00227   //we need to close the loop, so we also have to raytrace from the last pt to first pt
00228   new_x = x_i + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
00229   new_y = y_i + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
00230   if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
00231     return footprint_cells;
00232   }
00233   new_x = x_i + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
00234   new_y = y_i + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
00235   if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
00236     return footprint_cells;
00237   }
00238 
00239   getLineCells(x0, x1, y0, y1, footprint_cells);
00240 
00241   if(fill) {
00242     getFillCells(footprint_cells);
00243   }
00244 
00245   return footprint_cells;
00246 }
00247 
00248 } /* namespace base_local_planner */


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:38