costmap_2d.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, 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: Eitan Marder-Eppstein
00036  *         David V. Lu!!
00037  *********************************************************************/
00038 #include <costmap_2d/costmap_2d.h>
00039 #include <cstdio>
00040 
00041 using namespace std;
00042 
00043 namespace costmap_2d
00044 {
00045 Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
00046                      double origin_x, double origin_y, unsigned char default_value) :
00047     size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
00048     origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
00049 {
00050   access_ = new mutex_t();
00051 
00052   // create the costmap
00053   initMaps(size_x_, size_y_);
00054   resetMaps();
00055 }
00056 
00057 void Costmap2D::deleteMaps()
00058 {
00059   // clean up data
00060   boost::unique_lock<mutex_t> lock(*access_);
00061   delete[] costmap_;
00062   costmap_ = NULL;
00063 }
00064 
00065 void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
00066 {
00067   boost::unique_lock<mutex_t> lock(*access_);
00068   delete[] costmap_;
00069   costmap_ = new unsigned char[size_x * size_y];
00070 }
00071 
00072 void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
00073                           double origin_x, double origin_y)
00074 {
00075   size_x_ = size_x;
00076   size_y_ = size_y;
00077   resolution_ = resolution;
00078   origin_x_ = origin_x;
00079   origin_y_ = origin_y;
00080 
00081   initMaps(size_x, size_y);
00082 
00083   // reset our maps to have no information
00084   resetMaps();
00085 }
00086 
00087 void Costmap2D::resetMaps()
00088 {
00089   boost::unique_lock<mutex_t> lock(*access_);
00090   memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
00091 }
00092 
00093 void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
00094 {
00095   boost::unique_lock<mutex_t> lock(*(access_));
00096   unsigned int len = xn - x0;
00097   for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
00098     memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
00099 }
00100 
00101 bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
00102                                   double win_size_y)
00103 {
00104   // check for self windowing
00105   if (this == &map)
00106   {
00107     // ROS_ERROR("Cannot convert this costmap into a window of itself");
00108     return false;
00109   }
00110 
00111   // clean up old data
00112   deleteMaps();
00113 
00114   // compute the bounds of our new map
00115   unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
00116   if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
00117       || !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
00118   {
00119     // ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
00120     return false;
00121   }
00122 
00123   size_x_ = upper_right_x - lower_left_x;
00124   size_y_ = upper_right_y - lower_left_y;
00125   resolution_ = map.resolution_;
00126   origin_x_ = win_origin_x;
00127   origin_y_ = win_origin_y;
00128 
00129   // initialize our various maps and reset markers for inflation
00130   initMaps(size_x_, size_y_);
00131 
00132   // copy the window of the static map and the costmap that we're taking
00133   copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
00134   return true;
00135 }
00136 
00137 Costmap2D& Costmap2D::operator=(const Costmap2D& map)
00138 {
00139   // check for self assignement
00140   if (this == &map)
00141     return *this;
00142 
00143   // clean up old data
00144   deleteMaps();
00145 
00146   size_x_ = map.size_x_;
00147   size_y_ = map.size_y_;
00148   resolution_ = map.resolution_;
00149   origin_x_ = map.origin_x_;
00150   origin_y_ = map.origin_y_;
00151 
00152   // initialize our various maps
00153   initMaps(size_x_, size_y_);
00154 
00155   // copy the cost map
00156   memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
00157 
00158   return *this;
00159 }
00160 
00161 Costmap2D::Costmap2D(const Costmap2D& map) :
00162     costmap_(NULL)
00163 {
00164   access_ = new mutex_t();
00165   *this = map;
00166 }
00167 
00168 // just initialize everything to NULL by default
00169 Costmap2D::Costmap2D() :
00170     size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
00171 {
00172   access_ = new mutex_t();
00173 }
00174 
00175 Costmap2D::~Costmap2D()
00176 {
00177   deleteMaps();
00178   delete access_;
00179 }
00180 
00181 unsigned int Costmap2D::cellDistance(double world_dist)
00182 {
00183   double cells_dist = max(0.0, ceil(world_dist / resolution_));
00184   return (unsigned int)cells_dist;
00185 }
00186 
00187 unsigned char* Costmap2D::getCharMap() const
00188 {
00189   return costmap_;
00190 }
00191 
00192 unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
00193 {
00194   return costmap_[getIndex(mx, my)];
00195 }
00196 
00197 void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
00198 {
00199   costmap_[getIndex(mx, my)] = cost;
00200 }
00201 
00202 void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
00203 {
00204   wx = origin_x_ + (mx + 0.5) * resolution_;
00205   wy = origin_y_ + (my + 0.5) * resolution_;
00206 }
00207 
00208 bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
00209 {
00210   if (wx < origin_x_ || wy < origin_y_)
00211     return false;
00212 
00213   mx = (int)((wx - origin_x_) / resolution_);
00214   my = (int)((wy - origin_y_) / resolution_);
00215 
00216   if (mx < size_x_ && my < size_y_)
00217     return true;
00218 
00219   return false;
00220 }
00221 
00222 void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
00223 {
00224   mx = (int)((wx - origin_x_) / resolution_);
00225   my = (int)((wy - origin_y_) / resolution_);
00226 }
00227 
00228 void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
00229 {
00230   // Here we avoid doing any math to wx,wy before comparing them to
00231   // the bounds, so their values can go out to the max and min values
00232   // of double floating point.
00233   if (wx < origin_x_)
00234   {
00235     mx = 0;
00236   }
00237   else if (wx >= resolution_ * size_x_ + origin_x_)
00238   {
00239     mx = size_x_ - 1;
00240   }
00241   else
00242   {
00243     mx = (int)((wx - origin_x_) / resolution_);
00244   }
00245 
00246   if (wy < origin_y_)
00247   {
00248     my = 0;
00249   }
00250   else if (wy >= resolution_ * size_y_ + origin_y_)
00251   {
00252     my = size_y_ - 1;
00253   }
00254   else
00255   {
00256     my = (int)((wy - origin_y_) / resolution_);
00257   }
00258 }
00259 
00260 void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
00261 {
00262   // project the new origin into the grid
00263   int cell_ox, cell_oy;
00264   cell_ox = int((new_origin_x - origin_x_) / resolution_);
00265   cell_oy = int((new_origin_y - origin_y_) / resolution_);
00266 
00267   // compute the associated world coordinates for the origin cell
00268   // because we want to keep things grid-aligned
00269   double new_grid_ox, new_grid_oy;
00270   new_grid_ox = origin_x_ + cell_ox * resolution_;
00271   new_grid_oy = origin_y_ + cell_oy * resolution_;
00272 
00273   // To save casting from unsigned int to int a bunch of times
00274   int size_x = size_x_;
00275   int size_y = size_y_;
00276 
00277   // we need to compute the overlap of the new and existing windows
00278   int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
00279   lower_left_x = min(max(cell_ox, 0), size_x);
00280   lower_left_y = min(max(cell_oy, 0), size_y);
00281   upper_right_x = min(max(cell_ox + size_x, 0), size_x);
00282   upper_right_y = min(max(cell_oy + size_y, 0), size_y);
00283 
00284   unsigned int cell_size_x = upper_right_x - lower_left_x;
00285   unsigned int cell_size_y = upper_right_y - lower_left_y;
00286 
00287   // we need a map to store the obstacles in the window temporarily
00288   unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
00289 
00290   // copy the local window in the costmap to the local map
00291   copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
00292 
00293   // now we'll set the costmap to be completely unknown if we track unknown space
00294   resetMaps();
00295 
00296   // update the origin with the appropriate world coordinates
00297   origin_x_ = new_grid_ox;
00298   origin_y_ = new_grid_oy;
00299 
00300   // compute the starting cell location for copying data back in
00301   int start_x = lower_left_x - cell_ox;
00302   int start_y = lower_left_y - cell_oy;
00303 
00304   // now we want to copy the overlapping information back into the map, but in its new location
00305   copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00306 
00307   // make sure to clean up
00308   delete[] local_map;
00309 }
00310 
00311 bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
00312 {
00313   // we assume the polygon is given in the global_frame... we need to transform it to map coordinates
00314   std::vector<MapLocation> map_polygon;
00315   for (unsigned int i = 0; i < polygon.size(); ++i)
00316   {
00317     MapLocation loc;
00318     if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
00319     {
00320       // ("Polygon lies outside map bounds, so we can't fill it");
00321       return false;
00322     }
00323     map_polygon.push_back(loc);
00324   }
00325 
00326   std::vector<MapLocation> polygon_cells;
00327 
00328   // get the cells that fill the polygon
00329   convexFillCells(map_polygon, polygon_cells);
00330 
00331   // set the cost of those cells
00332   for (unsigned int i = 0; i < polygon_cells.size(); ++i)
00333   {
00334     unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
00335     costmap_[index] = cost_value;
00336   }
00337   return true;
00338 }
00339 
00340 void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
00341 {
00342   PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
00343   for (unsigned int i = 0; i < polygon.size() - 1; ++i)
00344   {
00345     raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
00346   }
00347   if (!polygon.empty())
00348   {
00349     unsigned int last_index = polygon.size() - 1;
00350     // we also need to close the polygon by going from the last point to the first
00351     raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
00352   }
00353 }
00354 
00355 void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
00356 {
00357   // we need a minimum polygon of a triangle
00358   if (polygon.size() < 3)
00359     return;
00360 
00361   // first get the cells that make up the outline of the polygon
00362   polygonOutlineCells(polygon, polygon_cells);
00363 
00364   // quick bubble sort to sort points by x
00365   MapLocation swap;
00366   unsigned int i = 0;
00367   while (i < polygon_cells.size() - 1)
00368   {
00369     if (polygon_cells[i].x > polygon_cells[i + 1].x)
00370     {
00371       swap = polygon_cells[i];
00372       polygon_cells[i] = polygon_cells[i + 1];
00373       polygon_cells[i + 1] = swap;
00374 
00375       if (i > 0)
00376         --i;
00377     }
00378     else
00379       ++i;
00380   }
00381 
00382   i = 0;
00383   MapLocation min_pt;
00384   MapLocation max_pt;
00385   unsigned int min_x = polygon_cells[0].x;
00386   unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
00387 
00388   // walk through each column and mark cells inside the polygon
00389   for (unsigned int x = min_x; x <= max_x; ++x)
00390   {
00391     if (i >= polygon_cells.size() - 1)
00392       break;
00393 
00394     if (polygon_cells[i].y < polygon_cells[i + 1].y)
00395     {
00396       min_pt = polygon_cells[i];
00397       max_pt = polygon_cells[i + 1];
00398     }
00399     else
00400     {
00401       min_pt = polygon_cells[i + 1];
00402       max_pt = polygon_cells[i];
00403     }
00404 
00405     i += 2;
00406     while (i < polygon_cells.size() && polygon_cells[i].x == x)
00407     {
00408       if (polygon_cells[i].y < min_pt.y)
00409         min_pt = polygon_cells[i];
00410       else if (polygon_cells[i].y > max_pt.y)
00411         max_pt = polygon_cells[i];
00412       ++i;
00413     }
00414 
00415     MapLocation pt;
00416     // loop though cells in the column
00417     for (unsigned int y = min_pt.y; y < max_pt.y; ++y)
00418     {
00419       pt.x = x;
00420       pt.y = y;
00421       polygon_cells.push_back(pt);
00422     }
00423   }
00424 }
00425 
00426 unsigned int Costmap2D::getSizeInCellsX() const
00427 {
00428   return size_x_;
00429 }
00430 
00431 unsigned int Costmap2D::getSizeInCellsY() const
00432 {
00433   return size_y_;
00434 }
00435 
00436 double Costmap2D::getSizeInMetersX() const
00437 {
00438   return (size_x_ - 1 + 0.5) * resolution_;
00439 }
00440 
00441 double Costmap2D::getSizeInMetersY() const
00442 {
00443   return (size_y_ - 1 + 0.5) * resolution_;
00444 }
00445 
00446 double Costmap2D::getOriginX() const
00447 {
00448   return origin_x_;
00449 }
00450 
00451 double Costmap2D::getOriginY() const
00452 {
00453   return origin_y_;
00454 }
00455 
00456 double Costmap2D::getResolution() const
00457 {
00458   return resolution_;
00459 }
00460 
00461 bool Costmap2D::saveMap(std::string file_name)
00462 {
00463   FILE *fp = fopen(file_name.c_str(), "w");
00464 
00465   if (!fp)
00466   {
00467     return false;
00468   }
00469 
00470   fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
00471   for (unsigned int iy = 0; iy < size_y_; iy++)
00472   {
00473     for (unsigned int ix = 0; ix < size_x_; ix++)
00474     {
00475       unsigned char cost = getCost(ix, iy);
00476       fprintf(fp, "%d ", cost);
00477     }
00478     fprintf(fp, "\n");
00479   }
00480   fclose(fp);
00481   return true;
00482 }
00483 
00484 }  // namespace costmap_2d


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15