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::addExtraBounds(double mx0, double my0, double mx1, double my1)
22 {
23  extra_min_x_ = std::min(mx0, extra_min_x_);
24  extra_max_x_ = std::max(mx1, extra_max_x_);
25  extra_min_y_ = std::min(my0, extra_min_y_);
26  extra_max_y_ = std::max(my1, extra_max_y_);
27  has_extra_bounds_ = true;
28 }
29 
30 void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y)
31 {
32  if (!has_extra_bounds_)
33  return;
34 
35  *min_x = std::min(extra_min_x_, *min_x);
36  *min_y = std::min(extra_min_y_, *min_y);
37  *max_x = std::max(extra_max_x_, *max_x);
38  *max_y = std::max(extra_max_y_, *max_y);
39  extra_min_x_ = 1e6;
40  extra_min_y_ = 1e6;
41  extra_max_x_ = -1e6;
42  extra_max_y_ = -1e6;
43  has_extra_bounds_ = false;
44 }
45 
46 void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
47 {
48  if (!enabled_)
49  return;
50 
51  unsigned char* master_array = master_grid.getCharMap();
52  unsigned int span = master_grid.getSizeInCellsX();
53 
54  for (int j = min_j; j < max_j; j++)
55  {
56  unsigned int it = j * span + min_i;
57  for (int i = min_i; i < max_i; i++)
58  {
59  if (costmap_[it] == NO_INFORMATION){
60  it++;
61  continue;
62  }
63 
64  unsigned char old_cost = master_array[it];
65  if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
66  master_array[it] = costmap_[it];
67  it++;
68  }
69  }
70 }
71 
72 void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
73  int max_i, int max_j)
74 {
75  if (!enabled_)
76  return;
77  unsigned char* master = master_grid.getCharMap();
78  unsigned int span = master_grid.getSizeInCellsX();
79 
80  for (int j = min_j; j < max_j; j++)
81  {
82  unsigned int it = span*j+min_i;
83  for (int i = min_i; i < max_i; i++)
84  {
85  master[it] = costmap_[it];
86  it++;
87  }
88  }
89 }
90 
91 void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
92 {
93  if (!enabled_)
94  return;
95  unsigned char* master = master_grid.getCharMap();
96  unsigned int span = master_grid.getSizeInCellsX();
97 
98  for (int j = min_j; j < max_j; j++)
99  {
100  unsigned int it = span*j+min_i;
101  for (int i = min_i; i < max_i; i++)
102  {
103  if (costmap_[it] != NO_INFORMATION)
104  master[it] = costmap_[it];
105  it++;
106  }
107  }
108 }
109 
110 void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
111 {
112  if (!enabled_)
113  return;
114  unsigned char* master_array = master_grid.getCharMap();
115  unsigned int span = master_grid.getSizeInCellsX();
116 
117  for (int j = min_j; j < max_j; j++)
118  {
119  unsigned int it = j * span + min_i;
120  for (int i = min_i; i < max_i; i++)
121  {
122  if (costmap_[it] == NO_INFORMATION){
123  it++;
124  continue;
125  }
126 
127  unsigned char old_cost = master_array[it];
128  if (old_cost == NO_INFORMATION)
129  master_array[it] = costmap_[it];
130  else
131  {
132  int sum = old_cost + costmap_[it];
134  master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
135  else
136  master_array[it] = sum;
137  }
138  it++;
139  }
140  }
141 }
142 } // namespace costmap_2d
LayeredCostmap * layered_costmap_
Definition: layer.h:122
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)
double getOriginX() const
Accessor for the x origin of the costmap.
Definition: costmap_2d.cpp:446
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.
double getOriginY() const
Accessor for the y origin of the costmap.
Definition: costmap_2d.cpp:451
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:431
bool enabled_
Currently this var is managed by subclasses. TODO: make this managed by this class and/or container c...
Definition: layer.h:124
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:426
static const unsigned char NO_INFORMATION
Definition: cost_values.h:42
void addExtraBounds(double mx0, double my0, double mx1, double my1)
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
double getResolution() const
Accessor for the resolution of the costmap.
Definition: costmap_2d.cpp:456
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 Sun Mar 3 2019 03:44:17