00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00034 #include <map>
00035
00036 #include <costmap_2d/costmap_2d.h>
00037 #include <costmap_2d/layered_costmap.h>
00038 #include <costmap_2d/obstacle_layer.h>
00039 #include <costmap_2d/inflation_layer.h>
00040 #include <costmap_2d/observation_buffer.h>
00041 #include <costmap_2d/testing_helper.h>
00042 #include <gtest/gtest.h>
00043 #include <tf/transform_listener.h>
00044
00045 using namespace costmap_2d;
00046 using geometry_msgs::Point;
00047
00048 std::vector<Point> setRadii(LayeredCostmap& layers, double length, double width, double inflation_radius)
00049 {
00050 std::vector<Point> polygon;
00051 Point p;
00052 p.x = width;
00053 p.y = length;
00054 polygon.push_back(p);
00055 p.x = width;
00056 p.y = -length;
00057 polygon.push_back(p);
00058 p.x = -width;
00059 p.y = -length;
00060 polygon.push_back(p);
00061 p.x = -width;
00062 p.y = length;
00063 polygon.push_back(p);
00064 layers.setFootprint(polygon);
00065
00066 ros::NodeHandle nh;
00067 nh.setParam("/inflation_tests/inflation/inflation_radius", inflation_radius);
00068
00069 return polygon;
00070 }
00071
00072
00073 void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap, InflationLayer* ilayer, double inflation_radius)
00074 {
00075 bool* seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
00076 memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
00077 std::map<double, std::vector<CellData> > m;
00078 CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
00079 m[0].push_back(initial);
00080 for (std::map<double, std::vector<CellData> >::iterator bin = m.begin(); bin != m.end(); ++bin)
00081 {
00082 for (int i = 0; i < bin->second.size(); ++i)
00083 {
00084 const CellData& cell = bin->second[i];
00085 if (!seen[cell.index_])
00086 {
00087 seen[cell.index_] = true;
00088 unsigned int dx = abs(cell.x_ - cell.src_x_);
00089 unsigned int dy = abs(cell.y_ - cell.src_y_);
00090 double dist = hypot(dx, dy);
00091
00092 unsigned char expected_cost = ilayer->computeCost(dist);
00093 ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);
00094
00095 if (dist > inflation_radius)
00096 {
00097 continue;
00098 }
00099
00100 if (cell.x_ > 0)
00101 {
00102 CellData data(costmap->getIndex(cell.x_-1, cell.y_),
00103 cell.x_-1, cell.y_, cell.src_x_, cell.src_y_);
00104 m[dist].push_back(data);
00105 }
00106 if (cell.y_ > 0)
00107 {
00108 CellData data(costmap->getIndex(cell.x_, cell.y_-1),
00109 cell.x_, cell.y_-1, cell.src_x_, cell.src_y_);
00110 m[dist].push_back(data);
00111 }
00112 if (cell.x_ < costmap->getSizeInCellsX() - 1)
00113 {
00114 CellData data(costmap->getIndex(cell.x_+1, cell.y_),
00115 cell.x_+1, cell.y_, cell.src_x_, cell.src_y_);
00116 m[dist].push_back(data);
00117 }
00118 if (cell.y_ < costmap->getSizeInCellsY() - 1)
00119 {
00120 CellData data(costmap->getIndex(cell.x_, cell.y_+1),
00121 cell.x_, cell.y_+1, cell.src_x_, cell.src_y_);
00122 m[dist].push_back(data);
00123 }
00124 }
00125 }
00126 }
00127 delete[] seen;
00128 }
00129
00130 TEST(costmap, testAdjacentToObstacleCanStillMove){
00131 tf::TransformListener tf;
00132 LayeredCostmap layers("frame", false, false);
00133 layers.resizeMap(10, 10, 1, 0, 0);
00134
00135
00136
00137 std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
00138
00139 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00140 InflationLayer* ilayer = addInflationLayer(layers, tf);
00141 layers.setFootprint(polygon);
00142
00143 addObservation(olayer, 0, 0, MAX_Z);
00144
00145 layers.updateMap(0,0,0);
00146 Costmap2D* costmap = layers.getCostmap();
00147
00148 EXPECT_EQ( LETHAL_OBSTACLE, costmap->getCost( 0, 0 ));
00149 EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 0 ));
00150 EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 2, 0 ));
00151 EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 3, 0 ));
00152 EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 2, 1 ));
00153 EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 1 ));
00154 }
00155
00156 TEST(costmap, testInflationShouldNotCreateUnknowns){
00157 tf::TransformListener tf;
00158 LayeredCostmap layers("frame", false, false);
00159 layers.resizeMap(10, 10, 1, 0, 0);
00160
00161
00162
00163 std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
00164
00165 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00166 InflationLayer* ilayer = addInflationLayer(layers, tf);
00167 layers.setFootprint(polygon);
00168
00169 addObservation(olayer, 0, 0, MAX_Z);
00170
00171 layers.updateMap(0,0,0);
00172 Costmap2D* costmap = layers.getCostmap();
00173
00174 EXPECT_EQ( countValues(*costmap, NO_INFORMATION), 0 );
00175 }
00176
00177
00181 TEST(costmap, testCostFunctionCorrectness){
00182 tf::TransformListener tf;
00183 LayeredCostmap layers("frame", false, false);
00184 layers.resizeMap(100, 100, 1, 0, 0);
00185
00186
00187
00188 std::vector<Point> polygon = setRadii(layers, 5.0, 6.25, 10.5);
00189
00190 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00191 InflationLayer* ilayer = addInflationLayer(layers, tf);
00192 layers.setFootprint(polygon);
00193
00194 addObservation(olayer, 50, 50, MAX_Z);
00195
00196 layers.updateMap(0,0,0);
00197 Costmap2D* map = layers.getCostmap();
00198
00199
00200
00201
00202
00203 for(unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++){
00204
00205 ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00206 ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00207
00208 ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00209 ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00210
00211 ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00212 ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00213
00214 ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00215 ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00216 }
00217
00218
00219 for(unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++){
00220 unsigned char expectedValue = ilayer->computeCost(i/1.0);
00221 ASSERT_EQ(map->getCost(50 + i, 50), expectedValue);
00222 }
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241 }
00242
00249 TEST(costmap, testInflationOrderCorrectness){
00250 tf::TransformListener tf;
00251 LayeredCostmap layers("frame", false, false);
00252 layers.resizeMap(10, 10, 1, 0, 0);
00253
00254
00255
00256 const double inflation_radius = 4.1;
00257 std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, inflation_radius);
00258
00259 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00260 InflationLayer* ilayer = addInflationLayer(layers, tf);
00261 layers.setFootprint(polygon);
00262
00263
00264
00265 addObservation(olayer, 4, 4, MAX_Z);
00266 addObservation(olayer, 5, 5, MAX_Z);
00267
00268 layers.updateMap(0, 0, 0);
00269
00270 validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
00271 validatePointInflation(5, 5, layers.getCostmap(), ilayer, inflation_radius);
00272 }
00273
00277 TEST(costmap, testInflation){
00278
00279 tf::TransformListener tf;
00280 LayeredCostmap layers("frame", false, false);
00281
00282
00283
00284 std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
00285
00286 addStaticLayer(layers, tf);
00287 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00288 InflationLayer* ilayer = addInflationLayer(layers, tf);
00289 layers.setFootprint(polygon);
00290
00291 Costmap2D* costmap = layers.getCostmap();
00292
00293 layers.updateMap(0,0,0);
00294
00295 ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)20);
00296 ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)28);
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307 addObservation(olayer, 0, 0, 0.4);
00308 layers.updateMap(0,0,0);
00309
00310
00311 ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51);
00312
00313
00314
00315 addObservation(olayer, 2, 0);
00316 layers.updateMap(0,0,0);
00317
00318
00319
00320
00321 ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)54);
00322
00323
00324 addObservation(olayer, 1, 9);
00325 layers.updateMap(0,0,0);
00326
00327 ASSERT_EQ(costmap->getCost(1, 9), LETHAL_OBSTACLE);
00328 ASSERT_EQ(costmap->getCost(0, 9), INSCRIBED_INFLATED_OBSTACLE);
00329 ASSERT_EQ(costmap->getCost(2, 9), INSCRIBED_INFLATED_OBSTACLE);
00330
00331
00332 addObservation(olayer, 0, 9);
00333 layers.updateMap(0,0,0);
00334
00335 ASSERT_EQ(costmap->getCost(0, 9), LETHAL_OBSTACLE);
00336 }
00337
00341 TEST(costmap, testInflation2){
00342
00343 tf::TransformListener tf;
00344 LayeredCostmap layers("frame", false, false);
00345
00346
00347
00348 std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
00349
00350 addStaticLayer(layers, tf);
00351 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00352 InflationLayer* ilayer = addInflationLayer(layers, tf);
00353 layers.setFootprint(polygon);
00354
00355
00356 addObservation(olayer, 1, 1, MAX_Z);
00357 addObservation(olayer, 2, 1, MAX_Z);
00358 addObservation(olayer, 2, 2, MAX_Z);
00359 layers.updateMap(0,0,0);
00360
00361 Costmap2D* costmap = layers.getCostmap();
00362
00363 ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
00364 ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
00365 }
00366
00370 TEST(costmap, testInflation3){
00371 tf::TransformListener tf;
00372 LayeredCostmap layers("frame", false, false);
00373 layers.resizeMap(10, 10, 1, 0, 0);
00374
00375
00376 std::vector<Point> polygon = setRadii(layers, 1, 1.75, 3);
00377
00378 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00379 InflationLayer* ilayer = addInflationLayer(layers, tf);
00380 layers.setFootprint(polygon);
00381
00382
00383 Costmap2D* costmap = layers.getCostmap();
00384 ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)0);
00385 ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)0);
00386 printMap(*costmap);
00387
00388 addObservation(olayer, 5, 5, MAX_Z);
00389 layers.updateMap(0,0,0);
00390 printMap(*costmap);
00391
00392
00393 ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29);
00394 ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1);
00395 ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4);
00396
00397
00398 layers.updateMap(0,0,0);
00399
00400 ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29);
00401 ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1);
00402 ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4);
00403 }
00404
00405
00406 int main(int argc, char** argv){
00407 ros::init(argc, argv, "inflation_tests");
00408 testing::InitGoogleTest(&argc, argv);
00409 return RUN_ALL_TESTS();
00410 }