costmap_2d.h
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 #ifndef COSTMAP_2D_COSTMAP_2D_H_
39 #define COSTMAP_2D_COSTMAP_2D_H_
40 
41 #include <vector>
42 #include <queue>
43 #include <geometry_msgs/Point.h>
44 #include <boost/thread.hpp>
45 
46 namespace costmap_2d
47 {
48 
49 // convenient for storing x/y point pairs
50 struct MapLocation
51 {
52  unsigned int x;
53  unsigned int y;
54 };
55 
60 class Costmap2D
61 {
62  friend class CostmapTester; // Need this for gtest to work correctly
63 public:
73  Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
74  double origin_x, double origin_y, unsigned char default_value = 0);
75 
80  Costmap2D(const Costmap2D& map);
81 
87  Costmap2D& operator=(const Costmap2D& map);
88 
97  bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
98  double win_size_y);
99 
103  Costmap2D();
104 
108  virtual ~Costmap2D();
109 
116  unsigned char getCost(unsigned int mx, unsigned int my) const;
117 
124  void setCost(unsigned int mx, unsigned int my, unsigned char cost);
125 
133  void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
134 
143  bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
144 
153  void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
154 
163  void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const;
164 
171  inline unsigned int getIndex(unsigned int mx, unsigned int my) const
172  {
173  return my * size_x_ + mx;
174  }
175 
182  inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
183  {
184  my = index / size_x_;
185  mx = index - (my * size_x_);
186  }
187 
192  unsigned char* getCharMap() const;
193 
198  unsigned int getSizeInCellsX() const;
199 
204  unsigned int getSizeInCellsY() const;
205 
210  double getSizeInMetersX() const;
211 
216  double getSizeInMetersY() const;
217 
222  double getOriginX() const;
223 
228  double getOriginY() const;
229 
234  double getResolution() const;
235 
236  void setDefaultValue(unsigned char c)
237  {
238  default_value_ = c;
239  }
240 
241  unsigned char getDefaultValue()
242  {
243  return default_value_;
244  }
245 
252  bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
253 
259  void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
260 
266  void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
267 
273  virtual void updateOrigin(double new_origin_x, double new_origin_y);
274 
279  bool saveMap(std::string file_name);
280 
281  void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
282  double origin_y);
283 
284  void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn);
285 
291  unsigned int cellDistance(double world_dist);
292 
293  // Provide a typedef to ease future code maintenance
294  typedef boost::recursive_mutex mutex_t;
295  mutex_t* getMutex()
296  {
297  return access_;
298  }
299 
300 protected:
314  template<typename data_type>
315  void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
316  unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
317  unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
318  unsigned int region_size_y)
319  {
320  // we'll first need to compute the starting points for each map
321  data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
322  data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
323 
324  // now, we'll copy the source map into the destination map
325  for (unsigned int i = 0; i < region_size_y; ++i)
326  {
327  memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
328  sm_index += sm_size_x;
329  dm_index += dm_size_x;
330  }
331  }
332 
336  virtual void deleteMaps();
337 
341  virtual void resetMaps();
342 
348  virtual void initMaps(unsigned int size_x, unsigned int size_y);
349 
359  template<class ActionType>
360  inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
361  unsigned int max_length = UINT_MAX)
362  {
363  int dx = x1 - x0;
364  int dy = y1 - y0;
365 
366  unsigned int abs_dx = abs(dx);
367  unsigned int abs_dy = abs(dy);
368 
369  int offset_dx = sign(dx);
370  int offset_dy = sign(dy) * size_x_;
371 
372  unsigned int offset = y0 * size_x_ + x0;
373 
374  // we need to chose how much to scale our dominant dimension, based on the maximum length of the line
375  double dist = hypot(dx, dy);
376  double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
377 
378  // if x is dominant
379  if (abs_dx >= abs_dy)
380  {
381  int error_y = abs_dx / 2;
382  bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
383  return;
384  }
385 
386  // otherwise y is dominant
387  int error_x = abs_dy / 2;
388  bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
389  }
390 
391 private:
395  template<class ActionType>
396  inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
397  int offset_b, unsigned int offset, unsigned int max_length)
398  {
399  unsigned int end = std::min(max_length, abs_da);
400  for (unsigned int i = 0; i < end; ++i)
401  {
402  at(offset);
403  offset += offset_a;
404  error_b += abs_db;
405  if ((unsigned int)error_b >= abs_da)
406  {
407  offset += offset_b;
408  error_b -= abs_da;
409  }
410  }
411  at(offset);
412  }
413 
414  inline int sign(int x)
415  {
416  return x > 0 ? 1.0 : -1.0;
417  }
418 
419  mutex_t* access_;
420 protected:
421  unsigned int size_x_;
422  unsigned int size_y_;
423  double resolution_;
424  double origin_x_;
425  double origin_y_;
426  unsigned char* costmap_;
427  unsigned char default_value_;
428 
429  class MarkCell
430  {
431  public:
432  MarkCell(unsigned char* costmap, unsigned char value) :
433  costmap_(costmap), value_(value)
434  {
435  }
436  inline void operator()(unsigned int offset)
437  {
438  costmap_[offset] = value_;
439  }
440  private:
441  unsigned char* costmap_;
442  unsigned char value_;
443  };
444 
445  class PolygonOutlineCells
446  {
447  public:
448  PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector<MapLocation>& cells) :
449  costmap_(costmap), char_map_(char_map), cells_(cells)
450  {
451  }
452 
453  // just push the relevant cells back onto the list
454  inline void operator()(unsigned int offset)
455  {
456  MapLocation loc;
457  costmap_.indexToCells(offset, loc.x, loc.y);
458  cells_.push_back(loc);
459  }
460 
461  private:
463  const unsigned char* char_map_;
464  std::vector<MapLocation>& cells_;
465  };
466 };
467 } // namespace costmap_2d
468 
469 #endif // COSTMAP_2D_COSTMAP_2D_H
costmap_2d::Costmap2D::getSizeInMetersX
double getSizeInMetersX() const
Accessor for the x size of the costmap in meters.
Definition: costmap_2d.cpp:440
costmap_2d::Costmap2D::sign
int sign(int x)
Definition: costmap_2d.h:450
costmap_2d::Costmap2D::initMaps
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
costmap_2d::Costmap2D::worldToMap
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
costmap_2d::Costmap2D::default_value_
unsigned char default_value_
Definition: costmap_2d.h:463
costmap_2d::Costmap2D::getSizeInMetersY
double getSizeInMetersY() const
Accessor for the y size of the costmap in meters.
Definition: costmap_2d.cpp:445
costmap_2d::Costmap2D::PolygonOutlineCells::costmap_
const Costmap2D & costmap_
Definition: costmap_2d.h:498
costmap_2d::Costmap2D::origin_x_
double origin_x_
Definition: costmap_2d.h:460
costmap_2d::Costmap2D::~Costmap2D
virtual ~Costmap2D()
Destructor.
Definition: costmap_2d.cpp:175
costmap_2d::Costmap2D::PolygonOutlineCells::cells_
std::vector< MapLocation > & cells_
Definition: costmap_2d.h:500
costmap_2d::Costmap2D
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:96
costmap_2d::Costmap2D::mapToWorld
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
costmap_2d::Costmap2D::getCharMap
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
Definition: costmap_2d.cpp:187
costmap_2d::Costmap2D::getOriginX
double getOriginX() const
Accessor for the x origin of the costmap.
Definition: costmap_2d.cpp:450
costmap_2d::Costmap2D::getDefaultValue
unsigned char getDefaultValue()
Definition: costmap_2d.h:277
costmap_2d::Costmap2D::CostmapTester
friend class CostmapTester
Definition: costmap_2d.h:98
costmap_2d::Costmap2D::Costmap2D
Costmap2D()
Default constructor.
Definition: costmap_2d.cpp:169
costmap_2d::Costmap2D::access_
mutex_t * access_
Definition: costmap_2d.h:455
costmap_2d::Costmap2D::polygonOutlineCells
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
costmap_2d::Costmap2D::operator=
Costmap2D & operator=(const Costmap2D &map)
Overloaded assignment operator.
Definition: costmap_2d.cpp:137
costmap_2d::MapLocation::y
unsigned int y
Definition: costmap_2d.h:125
costmap_2d::MapLocation::x
unsigned int x
Definition: costmap_2d.h:124
costmap_2d::Costmap2D::resizeMap
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
Definition: costmap_2d.cpp:72
costmap_2d::Costmap2D::setCost
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
costmap_2d::Costmap2D::MarkCell::operator()
void operator()(unsigned int offset)
Definition: costmap_2d.h:472
costmap_2d::Costmap2D::PolygonOutlineCells::operator()
void operator()(unsigned int offset)
Definition: costmap_2d.h:490
costmap_2d::Costmap2D::saveMap
bool saveMap(std::string file_name)
Save the costmap out to a pgm file.
Definition: costmap_2d.cpp:465
costmap_2d::MapLocation
Definition: costmap_2d.h:86
costmap_2d::Costmap2D::indexToCells
void indexToCells(unsigned int index, unsigned int &mx, unsigned int &my) const
Given an index... compute the associated map coordinates.
Definition: costmap_2d.h:218
costmap_2d::Costmap2D::PolygonOutlineCells::PolygonOutlineCells
PolygonOutlineCells(const Costmap2D &costmap, const unsigned char *char_map, std::vector< MapLocation > &cells)
Definition: costmap_2d.h:484
costmap_2d::Costmap2D::size_y_
unsigned int size_y_
Definition: costmap_2d.h:458
costmap_2d::Costmap2D::getSizeInCellsY
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:435
costmap_2d::Costmap2D::PolygonOutlineCells::char_map_
const unsigned char * char_map_
Definition: costmap_2d.h:499
costmap_2d::Costmap2D::resetMap
void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
Definition: costmap_2d.cpp:93
costmap_2d::Costmap2D::getMutex
mutex_t * getMutex()
Definition: costmap_2d.h:331
costmap_2d::Costmap2D::size_x_
unsigned int size_x_
Definition: costmap_2d.h:457
costmap_2d::Costmap2D::worldToMapEnforceBounds
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
costmap_2d::Costmap2D::getOriginY
double getOriginY() const
Accessor for the y origin of the costmap.
Definition: costmap_2d.cpp:455
costmap_2d::Costmap2D::copyCostmapWindow
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
costmap_2d::Costmap2D::cellDistance
unsigned int cellDistance(double world_dist)
Given distance in the world... convert it to cells.
Definition: costmap_2d.cpp:181
costmap_2d::Costmap2D::MarkCell::value_
unsigned char value_
Definition: costmap_2d.h:478
costmap_2d::Costmap2D::resolution_
double resolution_
Definition: costmap_2d.h:459
costmap_2d::Costmap2D::resetMaps
virtual void resetMaps()
Resets the costmap and static_map to be unknown space.
Definition: costmap_2d.cpp:87
costmap_2d::Costmap2D::deleteMaps
virtual void deleteMaps()
Deletes the costmap, static_map, and markers data structures.
Definition: costmap_2d.cpp:57
costmap_2d::Costmap2D::setDefaultValue
void setDefaultValue(unsigned char c)
Definition: costmap_2d.h:272
costmap_2d::Costmap2D::raytraceLine
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:396
costmap_2d::Costmap2D::copyMapRegion
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:351
costmap_2d::Costmap2D::origin_y_
double origin_y_
Definition: costmap_2d.h:461
costmap_2d::Costmap2D::getResolution
double getResolution() const
Accessor for the resolution of the costmap.
Definition: costmap_2d.cpp:460
costmap_2d::Costmap2D::updateOrigin
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
costmap_2d::Costmap2D::setConvexPolygonCost
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
costmap_2d::Costmap2D::getIndex
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.
Definition: costmap_2d.h:207
costmap_2d::Costmap2D::costmap_
unsigned char * costmap_
Definition: costmap_2d.h:462
costmap_2d::Costmap2D::getSizeInCellsX
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:430
costmap_2d::Costmap2D::bresenham2D
void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset, unsigned int max_length)
A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step.
Definition: costmap_2d.h:432
costmap_2d::Costmap2D::mutex_t
boost::recursive_mutex mutex_t
Definition: costmap_2d.h:330
costmap_2d::Costmap2D::worldToMapNoBounds
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
costmap_2d::Costmap2D::convexFillCells
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
costmap_2d::Costmap2D::MarkCell::costmap_
unsigned char * costmap_
Definition: costmap_2d.h:477
costmap_2d::Costmap2D::MarkCell::MarkCell
MarkCell(unsigned char *costmap, unsigned char value)
Definition: costmap_2d.h:468
costmap_2d::Costmap2D::getCost
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
Definition: costmap_2d.cpp:192
costmap_2d
Definition: array_parser.h:37


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:17