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
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 
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
mutex_t * getMutex()
Definition: costmap_2d.h:295
void indexToCells(unsigned int index, unsigned int &mx, unsigned int &my) const
Given an index... compute the associated map coordinates.
Definition: costmap_2d.h:182
unsigned int size_y_
Definition: costmap_2d.h:422
std::vector< MapLocation > & cells_
Definition: costmap_2d.h:464
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.
Definition: costmap_2d.h:171
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
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&#39;s raytracing algorithm... applies an action at each step...
Definition: costmap_2d.h:396
double sign(double x)
Return -1 if x < 0, +1 otherwise.
Definition: costmap_math.h:47
unsigned int size_x_
Definition: costmap_2d.h:421
void operator()(unsigned int offset)
Definition: costmap_2d.h:436
void setDefaultValue(unsigned char c)
Definition: costmap_2d.h:236
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
boost::recursive_mutex mutex_t
Definition: costmap_2d.h:294
PolygonOutlineCells(const Costmap2D &costmap, const unsigned char *char_map, std::vector< MapLocation > &cells)
Definition: costmap_2d.h:448
void operator()(unsigned int offset)
Definition: costmap_2d.h:454
MarkCell(unsigned char *costmap, unsigned char value)
Definition: costmap_2d.h:432
unsigned char default_value_
Definition: costmap_2d.h:427
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
unsigned char getDefaultValue()
Definition: costmap_2d.h:241
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


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