costmap_model.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 the Willow Garage 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: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include <costmap_model.h>
00038 
00039 using namespace std;
00040 using namespace costmap_2d;
00041 
00042 namespace iri_ackermann_local_planner {
00043   CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}
00044 
00045   double CostmapModel::footprintCost(const geometry_msgs::Point& position, const vector<geometry_msgs::Point>& footprint, 
00046       double inscribed_radius, double circumscribed_radius){
00047     //used to put things into grid coordinates
00048     unsigned int cell_x, cell_y;
00049 
00050     //get the cell coord of the center point of the robot
00051     if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
00052       return -1.0;
00053 
00054     //if number of points in the footprint is less than 3, we'll just assume a circular robot
00055     if(footprint.size() < 3){
00056       unsigned char cost = costmap_.getCost(cell_x, cell_y);
00057       //if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
00058       if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
00059         return -1.0;
00060       return cost;
00061     }
00062 
00063     //now we really have to lay down the footprint in the costmap grid
00064     unsigned int x0, x1, y0, y1;
00065     double line_cost = 0.0;
00066     double footprint_cost = 0.0;
00067 
00068     //we need to rasterize each line in the footprint
00069     for(unsigned int i = 0; i < footprint.size() - 1; ++i){
00070       //get the cell coord of the first point
00071       if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
00072         return -1.0;
00073 
00074       //get the cell coord of the second point
00075       if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
00076         return -1.0;
00077 
00078       line_cost = lineCost(x0, x1, y0, y1);
00079       footprint_cost = std::max(line_cost, footprint_cost);
00080 
00081       //if there is an obstacle that hits the line... we know that we can return false right away 
00082       if(line_cost < 0)
00083         return -1.0;
00084     }
00085 
00086     //we also need to connect the first point in the footprint to the last point
00087     //get the cell coord of the last point
00088     if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
00089       return -1.0;
00090 
00091     //get the cell coord of the first point
00092     if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
00093       return -1.0;
00094 
00095     line_cost = lineCost(x0, x1, y0, y1);
00096     footprint_cost = std::max(line_cost, footprint_cost);
00097 
00098     if(line_cost < 0)
00099       return -1.0;
00100 
00101     //if all line costs are legal... then we can return that the footprint is legal
00102     return footprint_cost;
00103 
00104   }
00105 
00106   //calculate the cost of a ray-traced line
00107   double CostmapModel::lineCost(int x0, int x1, 
00108       int y0, int y1){
00109     //Bresenham Ray-Tracing
00110     int deltax = abs(x1 - x0);        // The difference between the x's
00111     int deltay = abs(y1 - y0);        // The difference between the y's
00112     int x = x0;                       // Start x off at the first pixel
00113     int y = y0;                       // Start y off at the first pixel
00114 
00115     int xinc1, xinc2, yinc1, yinc2;
00116     int den, num, numadd, numpixels;
00117 
00118     double line_cost = 0.0;
00119     double point_cost = -1.0;
00120 
00121     if (x1 >= x0)                 // The x-values are increasing
00122     {
00123       xinc1 = 1;
00124       xinc2 = 1;
00125     }
00126     else                          // The x-values are decreasing
00127     {
00128       xinc1 = -1;
00129       xinc2 = -1;
00130     }
00131 
00132     if (y1 >= y0)                 // The y-values are increasing
00133     {
00134       yinc1 = 1;
00135       yinc2 = 1;
00136     }
00137     else                          // The y-values are decreasing
00138     {
00139       yinc1 = -1;
00140       yinc2 = -1;
00141     }
00142 
00143     if (deltax >= deltay)         // There is at least one x-value for every y-value
00144     {
00145       xinc1 = 0;                  // Don't change the x when numerator >= denominator
00146       yinc2 = 0;                  // Don't change the y for every iteration
00147       den = deltax;
00148       num = deltax / 2;
00149       numadd = deltay;
00150       numpixels = deltax;         // There are more x-values than y-values
00151     }
00152     else                          // There is at least one y-value for every x-value
00153     {
00154       xinc2 = 0;                  // Don't change the x for every iteration
00155       yinc1 = 0;                  // Don't change the y when numerator >= denominator
00156       den = deltay;
00157       num = deltay / 2;
00158       numadd = deltax;
00159       numpixels = deltay;         // There are more y-values than x-values
00160     }
00161 
00162     for (int curpixel = 0; curpixel <= numpixels; curpixel++)
00163     {
00164       point_cost = pointCost(x, y); //Score the current point
00165 
00166       if(point_cost < 0)
00167         return -1;
00168 
00169       if(line_cost < point_cost)
00170         line_cost = point_cost;
00171 
00172       num += numadd;              // Increase the numerator by the top of the fraction
00173       if (num >= den)             // Check if numerator >= denominator
00174       {
00175         num -= den;               // Calculate the new numerator value
00176         x += xinc1;               // Change the x as appropriate
00177         y += yinc1;               // Change the y as appropriate
00178       }
00179       x += xinc2;                 // Change the x as appropriate
00180       y += yinc2;                 // Change the y as appropriate
00181     }
00182 
00183     return line_cost;
00184   }
00185 
00186   double CostmapModel::pointCost(int x, int y){
00187     unsigned char cost = costmap_.getCost(x, y);
00188     //if the cell is in an obstacle the path is invalid
00189     //if(cost == LETHAL_OBSTACLE){
00190     if(cost == LETHAL_OBSTACLE || cost == NO_INFORMATION){
00191       return -1;
00192     }
00193 
00194     return cost;
00195   }
00196 
00197 };


iri_ackermann_local_planner
Author(s): Sergi Hernandez Juan
autogenerated on Fri Dec 6 2013 23:50:17