inflation_tests.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
34 #include <map>
35 #include <cmath>
36 
37 #include <costmap_2d/costmap_2d.h>
43 #include <gtest/gtest.h>
44 
45 using namespace costmap_2d;
46 using geometry_msgs::Point;
47 
48 std::vector<Point> setRadii(LayeredCostmap& layers, double length, double width, double inflation_radius)
49 {
50  std::vector<Point> polygon;
51  Point p;
52  p.x = width;
53  p.y = length;
54  polygon.push_back(p);
55  p.x = width;
56  p.y = -length;
57  polygon.push_back(p);
58  p.x = -width;
59  p.y = -length;
60  polygon.push_back(p);
61  p.x = -width;
62  p.y = length;
63  polygon.push_back(p);
64  layers.setFootprint(polygon);
65 
66  ros::NodeHandle nh;
67  nh.setParam("/inflation_tests/inflation/inflation_radius", inflation_radius);
68 
69  return polygon;
70 }
71 
72 // Test that a single point gets inflated properly
73 void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap, InflationLayer* ilayer, double inflation_radius)
74 {
75  bool* seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
76  memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
77  std::map<double, std::vector<CellData> > m;
78  CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
79  m[0].push_back(initial);
80  for (std::map<double, std::vector<CellData> >::iterator bin = m.begin(); bin != m.end(); ++bin)
81  {
82  for (int i = 0; i < bin->second.size(); ++i)
83  {
84  const CellData& cell = bin->second[i];
85  if (!seen[cell.index_])
86  {
87  seen[cell.index_] = true;
88  unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
89  unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
90  double dist = hypot(dx, dy);
91 
92  unsigned char expected_cost = ilayer->computeCost(dist);
93  ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);
94 
95  if (dist > inflation_radius)
96  {
97  continue;
98  }
99 
100  if (dist == bin->first)
101  {
102  // Adding to our current bin could cause a reallocation
103  // Which appears to cause the iterator to get messed up
104  dist += 0.001;
105  }
106 
107  if (cell.x_ > 0)
108  {
109  CellData data(costmap->getIndex(cell.x_-1, cell.y_),
110  cell.x_-1, cell.y_, cell.src_x_, cell.src_y_);
111  m[dist].push_back(data);
112  }
113  if (cell.y_ > 0)
114  {
115  CellData data(costmap->getIndex(cell.x_, cell.y_-1),
116  cell.x_, cell.y_-1, cell.src_x_, cell.src_y_);
117  m[dist].push_back(data);
118  }
119  if (cell.x_ < costmap->getSizeInCellsX() - 1)
120  {
121  CellData data(costmap->getIndex(cell.x_+1, cell.y_),
122  cell.x_+1, cell.y_, cell.src_x_, cell.src_y_);
123  m[dist].push_back(data);
124  }
125  if (cell.y_ < costmap->getSizeInCellsY() - 1)
126  {
127  CellData data(costmap->getIndex(cell.x_, cell.y_+1),
128  cell.x_, cell.y_+1, cell.src_x_, cell.src_y_);
129  m[dist].push_back(data);
130  }
131  }
132  }
133  }
134  delete[] seen;
135 }
136 
137 TEST(costmap, testAdjacentToObstacleCanStillMove){
139  LayeredCostmap layers("frame", false, false);
140  layers.resizeMap(10, 10, 1, 0, 0);
141 
142  // Footprint with inscribed radius = 2.1
143  // circumscribed radius = 3.1
144  std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
145 
146  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
147  InflationLayer* ilayer = addInflationLayer(layers, tf);
148  layers.setFootprint(polygon);
149 
150  addObservation(olayer, 0, 0, MAX_Z);
151 
152  layers.updateMap(0,0,0);
153  Costmap2D* costmap = layers.getCostmap();
154  //printMap(*costmap);
155  EXPECT_EQ( LETHAL_OBSTACLE, costmap->getCost( 0, 0 ));
156  EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 0 ));
157  EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 2, 0 ));
158  EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 3, 0 ));
159  EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 2, 1 ));
160  EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 1 ));
161 }
162 
163 TEST(costmap, testInflationShouldNotCreateUnknowns){
165  LayeredCostmap layers("frame", false, false);
166  layers.resizeMap(10, 10, 1, 0, 0);
167 
168  // Footprint with inscribed radius = 2.1
169  // circumscribed radius = 3.1
170  std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
171 
172  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
173  InflationLayer* ilayer = addInflationLayer(layers, tf);
174  layers.setFootprint(polygon);
175 
176  addObservation(olayer, 0, 0, MAX_Z);
177 
178  layers.updateMap(0,0,0);
179  Costmap2D* costmap = layers.getCostmap();
180 
181  EXPECT_EQ( countValues(*costmap, NO_INFORMATION), 0 );
182 }
183 
184 
188 TEST(costmap, testCostFunctionCorrectness){
190  LayeredCostmap layers("frame", false, false);
191  layers.resizeMap(100, 100, 1, 0, 0);
192 
193  // Footprint with inscribed radius = 5.0
194  // circumscribed radius = 8.0
195  std::vector<Point> polygon = setRadii(layers, 5.0, 6.25, 10.5);
196 
197  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
198  InflationLayer* ilayer = addInflationLayer(layers, tf);
199  layers.setFootprint(polygon);
200 
201  addObservation(olayer, 50, 50, MAX_Z);
202 
203  layers.updateMap(0,0,0);
204  Costmap2D* map = layers.getCostmap();
205 
206  // Verify that the circumscribed cost lower bound is as expected: based on the cost function.
207  //unsigned char c = ilayer->computeCost(8.0);
208  //ASSERT_EQ(ilayer->getCircumscribedCost(), c);
209 
210  for(unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++){
211  // To the right
212  ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
213  ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
214  // To the left
215  ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
216  ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
217  // Down
218  ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
219  ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
220  // Up
221  ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
222  ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
223  }
224 
225  // Verify the normalized cost attenuates as expected
226  for(unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++){
227  unsigned char expectedValue = ilayer->computeCost(i/1.0);
228  ASSERT_EQ(map->getCost(50 + i, 50), expectedValue);
229  }
230 
231  // Update with no hits. Should clear (revert to the static map
232  /*map->resetMapOutsideWindow(0, 0, 0.0, 0.0);
233  cloud.points.resize(0);
234 
235  p.x = 0.0;
236  p.y = 0.0;
237  p.z = MAX_Z;
238 
239  Observation obs2(p, cloud, 100.0, 100.0);
240  std::vector<Observation> obsBuf2;
241  obsBuf2.push_back(obs2);
242 
243  map->updateWorld(0, 0, obsBuf2, obsBuf2);
244 
245  for(unsigned int i = 0; i < 100; i++)
246  for(unsigned int j = 0; j < 100; j++)
247  ASSERT_EQ(map->getCost(i, j), costmap_2d::FREE_SPACE);*/
248 }
249 
256 TEST(costmap, testInflationOrderCorrectness){
258  LayeredCostmap layers("frame", false, false);
259  layers.resizeMap(10, 10, 1, 0, 0);
260 
261  // Footprint with inscribed radius = 2.1
262  // circumscribed radius = 3.1
263  const double inflation_radius = 4.1;
264  std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, inflation_radius);
265 
266  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
267  InflationLayer* ilayer = addInflationLayer(layers, tf);
268  layers.setFootprint(polygon);
269 
270  // Add two diagonal cells, they would induce problems under the
271  // previous implementations
272  addObservation(olayer, 4, 4, MAX_Z);
273  addObservation(olayer, 5, 5, MAX_Z);
274 
275  layers.updateMap(0, 0, 0);
276 
277  validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
278  validatePointInflation(5, 5, layers.getCostmap(), ilayer, inflation_radius);
279 }
280 
284 TEST(costmap, testInflation){
285 
287  LayeredCostmap layers("frame", false, false);
288 
289  // Footprint with inscribed radius = 2.1
290  // circumscribed radius = 3.1
291  std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
292 
293  addStaticLayer(layers, tf);
294  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
295  InflationLayer* ilayer = addInflationLayer(layers, tf);
296  layers.setFootprint(polygon);
297 
298  Costmap2D* costmap = layers.getCostmap();
299 
300  layers.updateMap(0,0,0);
301  //printMap(*costmap);
302  ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)20);
303  ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)28);
304 
305  /*/ Iterate over all id's and verify they are obstacles
306  for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
307  unsigned int ind = *it;
308  unsigned int x, y;
309  map.indexToCells(ind, x, y);
310  ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
311  ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
312  }*/
313 
314  addObservation(olayer, 0, 0, 0.4);
315  layers.updateMap(0,0,0);
316 
317  // It and its 2 neighbors makes 3 obstacles
318  ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51);
319 
320  // @todo Rewrite
321  // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
322  addObservation(olayer, 2, 0);
323  layers.updateMap(0,0,0);
324 
325  // Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from
326  // the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle
327  // at <0, 1>
328  ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)54);
329 
330  // Add an obstacle at <1, 9>. This will inflate obstacles around it
331  addObservation(olayer, 1, 9);
332  layers.updateMap(0,0,0);
333 
334  ASSERT_EQ(costmap->getCost(1, 9), LETHAL_OBSTACLE);
335  ASSERT_EQ(costmap->getCost(0, 9), INSCRIBED_INFLATED_OBSTACLE);
336  ASSERT_EQ(costmap->getCost(2, 9), INSCRIBED_INFLATED_OBSTACLE);
337 
338  // Add an obstacle and verify that it over-writes its inflated status
339  addObservation(olayer, 0, 9);
340  layers.updateMap(0,0,0);
341 
342  ASSERT_EQ(costmap->getCost(0, 9), LETHAL_OBSTACLE);
343 }
344 
348 TEST(costmap, testInflation2){
349 
351  LayeredCostmap layers("frame", false, false);
352 
353  // Footprint with inscribed radius = 2.1
354  // circumscribed radius = 3.1
355  std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
356 
357  addStaticLayer(layers, tf);
358  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
359  InflationLayer* ilayer = addInflationLayer(layers, tf);
360  layers.setFootprint(polygon);
361 
362  // Creat a small L-Shape all at once
363  addObservation(olayer, 1, 1, MAX_Z);
364  addObservation(olayer, 2, 1, MAX_Z);
365  addObservation(olayer, 2, 2, MAX_Z);
366  layers.updateMap(0,0,0);
367 
368  Costmap2D* costmap = layers.getCostmap();
369  //printMap(*costmap);
370  ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
371  ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
372 }
373 
377 TEST(costmap, testInflation3){
379  LayeredCostmap layers("frame", false, false);
380  layers.resizeMap(10, 10, 1, 0, 0);
381 
382  // 1 2 3
383  std::vector<Point> polygon = setRadii(layers, 1, 1.75, 3);
384 
385  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
386  InflationLayer* ilayer = addInflationLayer(layers, tf);
387  layers.setFootprint(polygon);
388 
389  // There should be no occupied cells
390  Costmap2D* costmap = layers.getCostmap();
391  ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)0);
392  ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)0);
393  printMap(*costmap);
394  // Add an obstacle at 5,5
395  addObservation(olayer, 5, 5, MAX_Z);
396  layers.updateMap(0,0,0);
397  printMap(*costmap);
398 
399  // Test fails because updated cell value is 0
400  ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29);
401  ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1);
402  ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4);
403 
404  // Update again - should see no change
405  layers.updateMap(0,0,0);
406 
407  ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29);
408  ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1);
409  ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4);
410 }
411 
412 
413 int main(int argc, char** argv){
414  ros::init(argc, argv, "inflation_tests");
415  testing::InitGoogleTest(&argc, argv);
416  return RUN_ALL_TESTS();
417 }
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
obstacle_layer.h
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
addObservation
void addObservation(costmap_2d::ObstacleLayer *olayer, double x, double y, double z=0.0, double ox=0.0, double oy=0.0, double oz=MAX_Z)
Definition: testing_helper.h:67
addStaticLayer
void addStaticLayer(costmap_2d::LayeredCostmap &layers, tf2_ros::Buffer &tf)
Definition: testing_helper.h:52
costmap_2d::INSCRIBED_INFLATED_OBSTACLE
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Definition: cost_values.h:79
costmap_2d::CellData::src_y_
unsigned int src_y_
Definition: inflation_layer.h:108
costmap_2d::LayeredCostmap::setFootprint
void setFootprint(const std::vector< geometry_msgs::Point > &footprint_spec)
Updates the stored footprint, updates the circumscribed and inscribed radii, and calls onFootprintCha...
Definition: layered_costmap.cpp:177
costmap_2d::NO_INFORMATION
static const unsigned char NO_INFORMATION
Definition: cost_values.h:77
costmap_2d::LayeredCostmap::resizeMap
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
Definition: layered_costmap.cpp:82
layered_costmap.h
costmap_2d::CellData
Storage for cell information used during obstacle inflation.
Definition: inflation_layer.h:90
costmap_2d::InflationLayer::computeCost
virtual unsigned char computeCost(double distance) const
Given a distance, compute a cost.
Definition: inflation_layer.h:140
costmap_2d::CellData::src_x_
unsigned int src_x_
Definition: inflation_layer.h:108
costmap_2d::Costmap2D
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:96
testing_helper.h
costmap_2d.h
MAX_Z
const double MAX_Z(1.0)
costmap_2d::CellData::index_
unsigned int index_
Definition: inflation_layer.h:106
costmap_2d::InflationLayer
Definition: inflation_layer.h:111
observation_buffer.h
Point
tf::Vector3 Point
addObstacleLayer
costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap &layers, tf2_ros::Buffer &tf)
Definition: testing_helper.h:59
setRadii
std::vector< Point > setRadii(LayeredCostmap &layers, double length, double width, double inflation_radius)
Definition: inflation_tests.cpp:48
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::ObstacleLayer
Definition: obstacle_layer.h:98
inflation_layer.h
costmap_2d::LETHAL_OBSTACLE
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:78
tf2_ros::Buffer
main
int main(int argc, char **argv)
Definition: inflation_tests.cpp:413
costmap_2d::CellData::x_
unsigned int x_
Definition: inflation_layer.h:107
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
costmap_2d::CellData::y_
unsigned int y_
Definition: inflation_layer.h:107
costmap_2d::LayeredCostmap::updateMap
void updateMap(double robot_x, double robot_y, double robot_yaw)
Update the underlying costmap with new data. If you want to update the map outside of the update loop...
Definition: layered_costmap.cpp:95
costmap_2d::LayeredCostmap
Instantiates different layer plugins and aggregates them into one score.
Definition: layered_costmap.h:91
tf
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::LayeredCostmap::getCostmap
Costmap2D * getCostmap()
Definition: layered_costmap.h:128
costmap_2d::FREE_SPACE
static const unsigned char FREE_SPACE
Definition: cost_values.h:80
addInflationLayer
costmap_2d::InflationLayer * addInflationLayer(costmap_2d::LayeredCostmap &layers, tf2_ros::Buffer &tf)
Definition: testing_helper.h:89
TEST
TEST(costmap, testAdjacentToObstacleCanStillMove)
Definition: inflation_tests.cpp:137
costmap_2d::Costmap2D::getSizeInCellsX
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:430
printMap
void printMap(costmap_2d::Costmap2D &costmap)
Definition: testing_helper.h:26
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
countValues
unsigned int countValues(costmap_2d::Costmap2D &costmap, unsigned char value, bool equal=true)
Definition: testing_helper.h:37
costmap_2d
Definition: array_parser.h:37
validatePointInflation
void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D *costmap, InflationLayer *ilayer, double inflation_radius)
Definition: inflation_tests.cpp:73
ros::NodeHandle


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