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 #ifndef COSTMAP_2D_COSTMAP_2D_H_
00039 #define COSTMAP_2D_COSTMAP_2D_H_
00040
00041 #include <vector>
00042 #include <queue>
00043 #include <geometry_msgs/Point.h>
00044 #include <boost/thread.hpp>
00045
00046 namespace costmap_2d
00047 {
00048
00049
00050 struct MapLocation
00051 {
00052 unsigned int x;
00053 unsigned int y;
00054 };
00055
00060 class Costmap2D
00061 {
00062 friend class CostmapTester;
00063 public:
00073 Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
00074 double origin_x, double origin_y, unsigned char default_value = 0);
00075
00080 Costmap2D(const Costmap2D& map);
00081
00087 Costmap2D& operator=(const Costmap2D& map);
00088
00097 bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
00098 double win_size_y);
00099
00103 Costmap2D();
00104
00108 virtual ~Costmap2D();
00109
00116 unsigned char getCost(unsigned int mx, unsigned int my) const;
00117
00124 void setCost(unsigned int mx, unsigned int my, unsigned char cost);
00125
00133 void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
00134
00143 bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
00144
00153 void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
00154
00163 void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const;
00164
00171 inline unsigned int getIndex(unsigned int mx, unsigned int my) const
00172 {
00173 return my * size_x_ + mx;
00174 }
00175
00182 inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
00183 {
00184 my = index / size_x_;
00185 mx = index - (my * size_x_);
00186 }
00187
00192 unsigned char* getCharMap() const;
00193
00198 unsigned int getSizeInCellsX() const;
00199
00204 unsigned int getSizeInCellsY() const;
00205
00210 double getSizeInMetersX() const;
00211
00216 double getSizeInMetersY() const;
00217
00222 double getOriginX() const;
00223
00228 double getOriginY() const;
00229
00234 double getResolution() const;
00235
00236 void setDefaultValue(unsigned char c)
00237 {
00238 default_value_ = c;
00239 }
00240
00241 unsigned char getDefaultValue()
00242 {
00243 return default_value_;
00244 }
00245
00252 bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
00253
00259 void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
00260
00266 void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
00267
00273 virtual void updateOrigin(double new_origin_x, double new_origin_y);
00274
00279 bool saveMap(std::string file_name);
00280
00281 void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
00282 double origin_y);
00283
00284 void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn);
00285
00291 unsigned int cellDistance(double world_dist);
00292
00293
00294 typedef boost::recursive_mutex mutex_t;
00295 mutex_t* getMutex()
00296 {
00297 return access_;
00298 }
00299
00300 protected:
00314 template<typename data_type>
00315 void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
00316 unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
00317 unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
00318 unsigned int region_size_y)
00319 {
00320
00321 data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
00322 data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
00323
00324
00325 for (unsigned int i = 0; i < region_size_y; ++i)
00326 {
00327 memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
00328 sm_index += sm_size_x;
00329 dm_index += dm_size_x;
00330 }
00331 }
00332
00336 virtual void deleteMaps();
00337
00341 virtual void resetMaps();
00342
00348 virtual void initMaps(unsigned int size_x, unsigned int size_y);
00349
00359 template<class ActionType>
00360 inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
00361 unsigned int max_length = UINT_MAX)
00362 {
00363 int dx = x1 - x0;
00364 int dy = y1 - y0;
00365
00366 unsigned int abs_dx = abs(dx);
00367 unsigned int abs_dy = abs(dy);
00368
00369 int offset_dx = sign(dx);
00370 int offset_dy = sign(dy) * size_x_;
00371
00372 unsigned int offset = y0 * size_x_ + x0;
00373
00374
00375 double dist = hypot(dx, dy);
00376 double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
00377
00378
00379 if (abs_dx >= abs_dy)
00380 {
00381 int error_y = abs_dx / 2;
00382 bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
00383 return;
00384 }
00385
00386
00387 int error_x = abs_dy / 2;
00388 bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
00389 }
00390
00391 private:
00395 template<class ActionType>
00396 inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
00397 int offset_b, unsigned int offset, unsigned int max_length)
00398 {
00399 unsigned int end = std::min(max_length, abs_da);
00400 for (unsigned int i = 0; i < end; ++i)
00401 {
00402 at(offset);
00403 offset += offset_a;
00404 error_b += abs_db;
00405 if ((unsigned int)error_b >= abs_da)
00406 {
00407 offset += offset_b;
00408 error_b -= abs_da;
00409 }
00410 }
00411 at(offset);
00412 }
00413
00414 inline int sign(int x)
00415 {
00416 return x > 0 ? 1.0 : -1.0;
00417 }
00418
00419 mutex_t* access_;
00420 protected:
00421 unsigned int size_x_;
00422 unsigned int size_y_;
00423 double resolution_;
00424 double origin_x_;
00425 double origin_y_;
00426 unsigned char* costmap_;
00427 unsigned char default_value_;
00428
00429 class MarkCell
00430 {
00431 public:
00432 MarkCell(unsigned char* costmap, unsigned char value) :
00433 costmap_(costmap), value_(value)
00434 {
00435 }
00436 inline void operator()(unsigned int offset)
00437 {
00438 costmap_[offset] = value_;
00439 }
00440 private:
00441 unsigned char* costmap_;
00442 unsigned char value_;
00443 };
00444
00445 class PolygonOutlineCells
00446 {
00447 public:
00448 PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector<MapLocation>& cells) :
00449 costmap_(costmap), char_map_(char_map), cells_(cells)
00450 {
00451 }
00452
00453
00454 inline void operator()(unsigned int offset)
00455 {
00456 MapLocation loc;
00457 costmap_.indexToCells(offset, loc.x, loc.y);
00458 cells_.push_back(loc);
00459 }
00460
00461 private:
00462 const Costmap2D& costmap_;
00463 const unsigned char* char_map_;
00464 std::vector<MapLocation>& cells_;
00465 };
00466 };
00467 }
00468
00469 #endif // COSTMAP_2D_COSTMAP_2D_H