costmap_layer.cpp
Go to the documentation of this file.
2 
3 namespace costmap_2d
4 {
5 
6 void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
7 {
8  *min_x = std::min(x, *min_x);
9  *min_y = std::min(y, *min_y);
10  *max_x = std::max(x, *max_x);
11  *max_y = std::max(y, *max_y);
12 }
13 
15 {
17  resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
18  master->getOriginX(), master->getOriginY());
19 }
20 
21 void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y)
22 {
23  unsigned char* grid = getCharMap();
24  for(int x=0; x<(int)getSizeInCellsX(); x++){
25  bool xrange = x>start_x && x<end_x;
26 
27  for(int y=0; y<(int)getSizeInCellsY(); y++){
28  if(xrange && y>start_y && y<end_y)
29  continue;
30  int index = getIndex(x,y);
31  if(grid[index]!=NO_INFORMATION){
32  grid[index] = NO_INFORMATION;
33  }
34  }
35  }
36 }
37 
38 void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
39 {
40  extra_min_x_ = std::min(mx0, extra_min_x_);
41  extra_max_x_ = std::max(mx1, extra_max_x_);
42  extra_min_y_ = std::min(my0, extra_min_y_);
43  extra_max_y_ = std::max(my1, extra_max_y_);
44  has_extra_bounds_ = true;
45 }
46 
47 void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y)
48 {
49  if (!has_extra_bounds_)
50  return;
51 
52  *min_x = std::min(extra_min_x_, *min_x);
53  *min_y = std::min(extra_min_y_, *min_y);
54  *max_x = std::max(extra_max_x_, *max_x);
55  *max_y = std::max(extra_max_y_, *max_y);
56  extra_min_x_ = 1e6;
57  extra_min_y_ = 1e6;
58  extra_max_x_ = -1e6;
59  extra_max_y_ = -1e6;
60  has_extra_bounds_ = false;
61 }
62 
63 void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
64 {
65  if (!enabled_)
66  return;
67 
68  unsigned char* master_array = master_grid.getCharMap();
69  unsigned int span = master_grid.getSizeInCellsX();
70 
71  for (int j = min_j; j < max_j; j++)
72  {
73  unsigned int it = j * span + min_i;
74  for (int i = min_i; i < max_i; i++)
75  {
76  if (costmap_[it] == NO_INFORMATION){
77  it++;
78  continue;
79  }
80 
81  unsigned char old_cost = master_array[it];
82  if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
83  master_array[it] = costmap_[it];
84  it++;
85  }
86  }
87 }
88 
89 void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
90  int max_i, int max_j)
91 {
92  if (!enabled_)
93  return;
94  unsigned char* master = master_grid.getCharMap();
95  unsigned int span = master_grid.getSizeInCellsX();
96 
97  for (int j = min_j; j < max_j; j++)
98  {
99  unsigned int it = span*j+min_i;
100  for (int i = min_i; i < max_i; i++)
101  {
102  master[it] = costmap_[it];
103  it++;
104  }
105  }
106 }
107 
108 void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
109 {
110  if (!enabled_)
111  return;
112  unsigned char* master = master_grid.getCharMap();
113  unsigned int span = master_grid.getSizeInCellsX();
114 
115  for (int j = min_j; j < max_j; j++)
116  {
117  unsigned int it = span*j+min_i;
118  for (int i = min_i; i < max_i; i++)
119  {
120  if (costmap_[it] != NO_INFORMATION)
121  master[it] = costmap_[it];
122  it++;
123  }
124  }
125 }
126 
127 void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
128 {
129  if (!enabled_)
130  return;
131  unsigned char* master_array = master_grid.getCharMap();
132  unsigned int span = master_grid.getSizeInCellsX();
133 
134  for (int j = min_j; j < max_j; j++)
135  {
136  unsigned int it = j * span + min_i;
137  for (int i = min_i; i < max_i; i++)
138  {
139  if (costmap_[it] == NO_INFORMATION){
140  it++;
141  continue;
142  }
143 
144  unsigned char old_cost = master_array[it];
145  if (old_cost == NO_INFORMATION)
146  master_array[it] = costmap_[it];
147  else
148  {
149  int sum = old_cost + costmap_[it];
151  master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
152  else
153  master_array[it] = sum;
154  }
155  it++;
156  }
157  }
158 }
159 } // namespace costmap_2d
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:430
LayeredCostmap * layered_costmap_
Definition: layer.h:121
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
Definition: costmap_2d.cpp:72
void updateWithOverwrite(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
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
double getOriginY() const
Accessor for the y origin of the costmap.
Definition: costmap_2d.cpp:455
void useExtraBounds(double *min_x, double *min_y, double *max_x, double *max_y)
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
Definition: costmap_2d.cpp:187
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Definition: cost_values.h:44
void updateWithTrueOverwrite(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
void updateWithAddition(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.
virtual void clearArea(int start_x, int start_y, int end_x, int end_y)
double getOriginX() const
Accessor for the x origin of the costmap.
Definition: costmap_2d.cpp:450
bool enabled_
Currently this var is managed by subclasses. TODO: make this managed by this class and/or container c...
Definition: layer.h:123
static const unsigned char NO_INFORMATION
Definition: cost_values.h:42
void addExtraBounds(double mx0, double my0, double mx1, double my1)
double getResolution() const
Accessor for the resolution of the costmap.
Definition: costmap_2d.cpp:460
void updateWithMax(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
void touch(double x, double y, double *min_x, double *min_y, double *max_x, double *max_y)
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