00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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
00053 initMaps(size_x_, size_y_);
00054 resetMaps();
00055 }
00056
00057 void Costmap2D::deleteMaps()
00058 {
00059
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
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
00105 if (this == &map)
00106 {
00107
00108 return false;
00109 }
00110
00111
00112 deleteMaps();
00113
00114
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
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
00130 initMaps(size_x_, size_y_);
00131
00132
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
00140 if (this == &map)
00141 return *this;
00142
00143
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
00153 initMaps(size_x_, size_y_);
00154
00155
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
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
00231
00232
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
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
00268
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
00274 int size_x = size_x_;
00275 int size_y = size_y_;
00276
00277
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
00288 unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
00289
00290
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
00294 resetMaps();
00295
00296
00297 origin_x_ = new_grid_ox;
00298 origin_y_ = new_grid_oy;
00299
00300
00301 int start_x = lower_left_x - cell_ox;
00302 int start_y = lower_left_y - cell_oy;
00303
00304
00305 copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00306
00307
00308 delete[] local_map;
00309 }
00310
00311 bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
00312 {
00313
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
00321 return false;
00322 }
00323 map_polygon.push_back(loc);
00324 }
00325
00326 std::vector<MapLocation> polygon_cells;
00327
00328
00329 convexFillCells(map_polygon, polygon_cells);
00330
00331
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
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
00358 if (polygon.size() < 3)
00359 return;
00360
00361
00362 polygonOutlineCells(polygon, polygon_cells);
00363
00364
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
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
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 }