costmap_2d.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Eitan Marder-Eppstein
36  * David V. Lu!!
37  *********************************************************************/
38 #include <costmap_2d/costmap_2d.h>
39 #include <cstdio>
40 
41 using namespace std;
42 
43 namespace costmap_2d
44 {
45 Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
46  double origin_x, double origin_y, unsigned char default_value) :
47  size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
48  origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
49 {
50  access_ = new mutex_t();
51 
52  // create the costmap
54  resetMaps();
55 }
56 
58 {
59  // clean up data
60  boost::unique_lock<mutex_t> lock(*access_);
61  delete[] costmap_;
62  costmap_ = NULL;
63 }
64 
65 void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
66 {
67  boost::unique_lock<mutex_t> lock(*access_);
68  delete[] costmap_;
69  costmap_ = new unsigned char[size_x * size_y];
70 }
71 
72 void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
73  double origin_x, double origin_y)
74 {
75  size_x_ = size_x;
76  size_y_ = size_y;
77  resolution_ = resolution;
78  origin_x_ = origin_x;
79  origin_y_ = origin_y;
80 
81  initMaps(size_x, size_y);
82 
83  // reset our maps to have no information
84  resetMaps();
85 }
86 
88 {
89  boost::unique_lock<mutex_t> lock(*access_);
90  memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
91 }
92 
93 void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
94 {
95  boost::unique_lock<mutex_t> lock(*(access_));
96  unsigned int len = xn - x0;
97  for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
98  memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
99 }
100 
101 bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
102  double win_size_y)
103 {
104  // check for self windowing
105  if (this == &map)
106  {
107  // ROS_ERROR("Cannot convert this costmap into a window of itself");
108  return false;
109  }
110 
111  // clean up old data
112  deleteMaps();
113 
114  // compute the bounds of our new map
115  unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
116  if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
117  || !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
118  {
119  // ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
120  return false;
121  }
122 
123  size_x_ = upper_right_x - lower_left_x;
124  size_y_ = upper_right_y - lower_left_y;
125  resolution_ = map.resolution_;
126  origin_x_ = win_origin_x;
127  origin_y_ = win_origin_y;
128 
129  // initialize our various maps and reset markers for inflation
131 
132  // copy the window of the static map and the costmap that we're taking
133  copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
134  return true;
135 }
136 
138 {
139  // check for self assignement
140  if (this == &map)
141  return *this;
142 
143  // clean up old data
144  deleteMaps();
145 
146  size_x_ = map.size_x_;
147  size_y_ = map.size_y_;
148  resolution_ = map.resolution_;
149  origin_x_ = map.origin_x_;
150  origin_y_ = map.origin_y_;
151 
152  // initialize our various maps
154 
155  // copy the cost map
156  memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
157 
158  return *this;
159 }
160 
162  costmap_(NULL)
163 {
164  access_ = new mutex_t();
165  *this = map;
166 }
167 
168 // just initialize everything to NULL by default
170  size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
171 {
172  access_ = new mutex_t();
173 }
174 
176 {
177  deleteMaps();
178  delete access_;
179 }
180 
181 unsigned int Costmap2D::cellDistance(double world_dist)
182 {
183  double cells_dist = max(0.0, ceil(world_dist / resolution_));
184  return (unsigned int)cells_dist;
185 }
186 
187 unsigned char* Costmap2D::getCharMap() const
188 {
189  return costmap_;
190 }
191 
192 unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
193 {
194  return costmap_[getIndex(mx, my)];
195 }
196 
197 void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
198 {
199  costmap_[getIndex(mx, my)] = cost;
200 }
201 
202 void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
203 {
204  wx = origin_x_ + (mx + 0.5) * resolution_;
205  wy = origin_y_ + (my + 0.5) * resolution_;
206 }
207 
208 bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
209 {
210  if (wx < origin_x_ || wy < origin_y_)
211  return false;
212 
213  mx = (int)((wx - origin_x_) / resolution_);
214  my = (int)((wy - origin_y_) / resolution_);
215 
216  if (mx < size_x_ && my < size_y_)
217  return true;
218 
219  return false;
220 }
221 
222 void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
223 {
224  mx = (int)((wx - origin_x_) / resolution_);
225  my = (int)((wy - origin_y_) / resolution_);
226 }
227 
228 void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
229 {
230  // Here we avoid doing any math to wx,wy before comparing them to
231  // the bounds, so their values can go out to the max and min values
232  // of double floating point.
233  if (wx < origin_x_)
234  {
235  mx = 0;
236  }
237  else if (wx >= resolution_ * size_x_ + origin_x_)
238  {
239  mx = size_x_ - 1;
240  }
241  else
242  {
243  mx = (int)((wx - origin_x_) / resolution_);
244  }
245 
246  if (wy < origin_y_)
247  {
248  my = 0;
249  }
250  else if (wy >= resolution_ * size_y_ + origin_y_)
251  {
252  my = size_y_ - 1;
253  }
254  else
255  {
256  my = (int)((wy - origin_y_) / resolution_);
257  }
258 }
259 
260 void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
261 {
262  // project the new origin into the grid
263  int cell_ox, cell_oy;
264  cell_ox = int((new_origin_x - origin_x_) / resolution_);
265  cell_oy = int((new_origin_y - origin_y_) / resolution_);
266 
267  // Nothing to update
268  if (cell_ox == 0 && cell_oy == 0)
269  return;
270 
271  // compute the associated world coordinates for the origin cell
272  // because we want to keep things grid-aligned
273  double new_grid_ox, new_grid_oy;
274  new_grid_ox = origin_x_ + cell_ox * resolution_;
275  new_grid_oy = origin_y_ + cell_oy * resolution_;
276 
277  // To save casting from unsigned int to int a bunch of times
278  int size_x = size_x_;
279  int size_y = size_y_;
280 
281  // we need to compute the overlap of the new and existing windows
282  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
283  lower_left_x = min(max(cell_ox, 0), size_x);
284  lower_left_y = min(max(cell_oy, 0), size_y);
285  upper_right_x = min(max(cell_ox + size_x, 0), size_x);
286  upper_right_y = min(max(cell_oy + size_y, 0), size_y);
287 
288  unsigned int cell_size_x = upper_right_x - lower_left_x;
289  unsigned int cell_size_y = upper_right_y - lower_left_y;
290 
291  // we need a map to store the obstacles in the window temporarily
292  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
293 
294  // copy the local window in the costmap to the local map
295  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
296 
297  // now we'll set the costmap to be completely unknown if we track unknown space
298  resetMaps();
299 
300  // update the origin with the appropriate world coordinates
301  origin_x_ = new_grid_ox;
302  origin_y_ = new_grid_oy;
303 
304  // compute the starting cell location for copying data back in
305  int start_x = lower_left_x - cell_ox;
306  int start_y = lower_left_y - cell_oy;
307 
308  // now we want to copy the overlapping information back into the map, but in its new location
309  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
310 
311  // make sure to clean up
312  delete[] local_map;
313 }
314 
315 bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
316 {
317  // we assume the polygon is given in the global_frame... we need to transform it to map coordinates
318  std::vector<MapLocation> map_polygon;
319  for (unsigned int i = 0; i < polygon.size(); ++i)
320  {
321  MapLocation loc;
322  if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
323  {
324  // ("Polygon lies outside map bounds, so we can't fill it");
325  return false;
326  }
327  map_polygon.push_back(loc);
328  }
329 
330  std::vector<MapLocation> polygon_cells;
331 
332  // get the cells that fill the polygon
333  convexFillCells(map_polygon, polygon_cells);
334 
335  // set the cost of those cells
336  for (unsigned int i = 0; i < polygon_cells.size(); ++i)
337  {
338  unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
339  costmap_[index] = cost_value;
340  }
341  return true;
342 }
343 
344 void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
345 {
346  PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
347  for (unsigned int i = 0; i < polygon.size() - 1; ++i)
348  {
349  raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
350  }
351  if (!polygon.empty())
352  {
353  unsigned int last_index = polygon.size() - 1;
354  // we also need to close the polygon by going from the last point to the first
355  raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
356  }
357 }
358 
359 void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
360 {
361  // we need a minimum polygon of a triangle
362  if (polygon.size() < 3)
363  return;
364 
365  // first get the cells that make up the outline of the polygon
366  polygonOutlineCells(polygon, polygon_cells);
367 
368  // quick bubble sort to sort points by x
370  unsigned int i = 0;
371  while (i < polygon_cells.size() - 1)
372  {
373  if (polygon_cells[i].x > polygon_cells[i + 1].x)
374  {
375  swap = polygon_cells[i];
376  polygon_cells[i] = polygon_cells[i + 1];
377  polygon_cells[i + 1] = swap;
378 
379  if (i > 0)
380  --i;
381  }
382  else
383  ++i;
384  }
385 
386  i = 0;
387  MapLocation min_pt;
388  MapLocation max_pt;
389  unsigned int min_x = polygon_cells[0].x;
390  unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
391 
392  // walk through each column and mark cells inside the polygon
393  for (unsigned int x = min_x; x <= max_x; ++x)
394  {
395  if (i >= polygon_cells.size() - 1)
396  break;
397 
398  if (polygon_cells[i].y < polygon_cells[i + 1].y)
399  {
400  min_pt = polygon_cells[i];
401  max_pt = polygon_cells[i + 1];
402  }
403  else
404  {
405  min_pt = polygon_cells[i + 1];
406  max_pt = polygon_cells[i];
407  }
408 
409  i += 2;
410  while (i < polygon_cells.size() && polygon_cells[i].x == x)
411  {
412  if (polygon_cells[i].y < min_pt.y)
413  min_pt = polygon_cells[i];
414  else if (polygon_cells[i].y > max_pt.y)
415  max_pt = polygon_cells[i];
416  ++i;
417  }
418 
419  MapLocation pt;
420  // loop though cells in the column
421  for (unsigned int y = min_pt.y; y < max_pt.y; ++y)
422  {
423  pt.x = x;
424  pt.y = y;
425  polygon_cells.push_back(pt);
426  }
427  }
428 }
429 
430 unsigned int Costmap2D::getSizeInCellsX() const
431 {
432  return size_x_;
433 }
434 
435 unsigned int Costmap2D::getSizeInCellsY() const
436 {
437  return size_y_;
438 }
439 
441 {
442  return (size_x_ - 1 + 0.5) * resolution_;
443 }
444 
446 {
447  return (size_y_ - 1 + 0.5) * resolution_;
448 }
449 
450 double Costmap2D::getOriginX() const
451 {
452  return origin_x_;
453 }
454 
455 double Costmap2D::getOriginY() const
456 {
457  return origin_y_;
458 }
459 
461 {
462  return resolution_;
463 }
464 
465 bool Costmap2D::saveMap(std::string file_name)
466 {
467  FILE *fp = fopen(file_name.c_str(), "w");
468 
469  if (!fp)
470  {
471  return false;
472  }
473 
474  fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
475  for (unsigned int iy = 0; iy < size_y_; iy++)
476  {
477  for (unsigned int ix = 0; ix < size_x_; ix++)
478  {
479  unsigned char cost = getCost(ix, iy);
480  fprintf(fp, "%d ", cost);
481  }
482  fprintf(fp, "\n");
483  }
484  fclose(fp);
485  return true;
486 }
487 
488 } // namespace costmap_2d
double getSizeInMetersX() const
Accessor for the x size of the costmap in meters.
Definition: costmap_2d.cpp:440
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Set the cost of a cell in the costmap.
Definition: costmap_2d.cpp:197
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:430
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
Definition: costmap_2d.cpp:72
virtual void initMaps(unsigned int size_x, unsigned int size_y)
Initializes the costmap, static_map, and markers data structures.
Definition: costmap_2d.cpp:65
unsigned int cellDistance(double world_dist)
Given distance in the world... convert it to cells.
Definition: costmap_2d.cpp:181
unsigned int size_y_
Definition: costmap_2d.h:422
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:435
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.
Definition: costmap_2d.h:171
void worldToMapEnforceBounds(double wx, double wy, int &mx, int &my) const
Convert from world coordinates to map coordinates, constraining results to legal bounds.
Definition: costmap_2d.cpp:228
double getOriginY() const
Accessor for the y origin of the costmap.
Definition: costmap_2d.cpp:455
unsigned int size_x_
Definition: costmap_2d.h:421
virtual void deleteMaps()
Deletes the costmap, static_map, and markers data structures.
Definition: costmap_2d.cpp:57
double getSizeInMetersY() const
Accessor for the y size of the costmap in meters.
Definition: costmap_2d.cpp:445
void copyMapRegion(data_type *source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x, data_type *dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y)
Copy a region of a source map into a destination map.
Definition: costmap_2d.h:315
void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length=UINT_MAX)
Raytrace a line and apply some action at each step.
Definition: costmap_2d.h:360
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
Definition: costmap_2d.cpp:187
boost::recursive_mutex mutex_t
Definition: costmap_2d.h:294
void convexFillCells(const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
Get the map cells that fill a convex polygon.
Definition: costmap_2d.cpp:359
bool saveMap(std::string file_name)
Save the costmap out to a pgm file.
Definition: costmap_2d.cpp:465
virtual void resetMaps()
Resets the costmap and static_map to be unknown space.
Definition: costmap_2d.cpp:87
void polygonOutlineCells(const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
Get the map cells that make up the outline of a polygon.
Definition: costmap_2d.cpp:344
bool setConvexPolygonCost(const std::vector< geometry_msgs::Point > &polygon, unsigned char cost_value)
Sets the cost of a convex polygon to a desired value.
Definition: costmap_2d.cpp:315
double min(double a, double b)
Costmap2D & operator=(const Costmap2D &map)
Overloaded assignment operator.
Definition: costmap_2d.cpp:137
virtual ~Costmap2D()
Destructor.
Definition: costmap_2d.cpp:175
unsigned char default_value_
Definition: costmap_2d.h:427
double getOriginX() const
Accessor for the x origin of the costmap.
Definition: costmap_2d.cpp:450
bool copyCostmapWindow(const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y)
Turn this costmap into a copy of a window of a costmap passed in.
Definition: costmap_2d.cpp:101
Costmap2D()
Default constructor.
Definition: costmap_2d.cpp:169
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Convert from world coordinates to map coordinates.
Definition: costmap_2d.cpp:208
double getResolution() const
Accessor for the resolution of the costmap.
Definition: costmap_2d.cpp:460
virtual void updateOrigin(double new_origin_x, double new_origin_y)
Move the origin of the costmap to a new location.... keeping data when it can.
Definition: costmap_2d.cpp:260
void worldToMapNoBounds(double wx, double wy, int &mx, int &my) const
Convert from world coordinates to map coordinates without checking for legal bounds.
Definition: costmap_2d.cpp:222
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
unsigned char * costmap_
Definition: costmap_2d.h:426
void swap(scoped_ptr< T > &, scoped_ptr< T > &)
double max(double a, double b)
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
Convert from map coordinates to world coordinates.
Definition: costmap_2d.cpp:202
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
Definition: costmap_2d.cpp:192
void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
Definition: costmap_2d.cpp:93


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Jun 22 2022 02:07:03