TEST(costmap, testAdjacentToObstacleCanStillMove){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
addObservation(olayer, 0, 0, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
//printMap(*costmap);
EXPECT_EQ( LETHAL_OBSTACLE, costmap->getCost( 0, 0 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 0 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 2, 0 ));
EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 3, 0 ));
EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 2, 1 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 1 ));
}
/**
* Test inflation for both static and dynamic obstacles
*/
TEST(costmap, testInflation){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
Costmap2D* costmap = layers.getCostmap();
layers.updateMap(0,0,0);
//printMap(*costmap);
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)20);
ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)28);
/*/ Iterate over all id's and verify they are obstacles
for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
unsigned int ind = *it;
unsigned int x, y;
map.indexToCells(ind, x, y);
ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
}*/
addObservation(olayer, 0, 0, 0.4);
layers.updateMap(0,0,0);
// It and its 2 neighbors makes 3 obstacles
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51);
// @todo Rewrite
// Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
addObservation(olayer, 2, 0);
layers.updateMap(0,0,0);
// Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from
// the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle
// at <0, 1>
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)54);
// Add an obstacle at <1, 9>. This will inflate obstacles around it
addObservation(olayer, 1, 9);
layers.updateMap(0,0,0);
ASSERT_EQ(costmap->getCost(1, 9), LETHAL_OBSTACLE);
ASSERT_EQ(costmap->getCost(0, 9), INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(2, 9), INSCRIBED_INFLATED_OBSTACLE);
// Add an obstacle and verify that it over-writes its inflated status
addObservation(olayer, 0, 9);
layers.updateMap(0,0,0);
ASSERT_EQ(costmap->getCost(0, 9), LETHAL_OBSTACLE);
}
/**
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
TEST(costmap, testInflation2){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
// Creat a small L-Shape all at once
addObservation(olayer, 1, 1, MAX_Z);
addObservation(olayer, 2, 1, MAX_Z);
addObservation(olayer, 2, 2, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
//printMap(*costmap);
ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
}
/** Test for copying a window of a costmap */
TEST(costmap, testWindowCopy){
Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
/*
for(unsigned int i = 0; i < 10; ++i){
for(unsigned int j = 0; j < 10; ++j){
printf("%3d ", map.getCost(i, j));
}
printf("\n");
}
printf("\n");
*/
Costmap2D windowCopy;
//first test that if we try to make a window that is too big, things fail
windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 12.0);
ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)0);
ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)0);
//Next, test that trying to make a map window itself fails
map.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10);
ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
//Next, test that legal input generates the result that we expect
windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6);
ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6);
//check that we actually get the windo that we expect
for(unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i){
for(unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j){
ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2));
//printf("%3d ", windowCopy.getCost(i, j));
}
//printf("\n");
}
}
/**
* Test for the cost function correctness with a larger range and different values
*/
TEST(costmap, testCostFunctionCorrectness){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(100, 100, 1, 0, 0);
// Footprint with inscribed radius = 5.0
// circumscribed radius = 8.0
std::vector<Point> polygon = setRadii(layers, 5.0, 6.25, 10.5);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
addObservation(olayer, 50, 50, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* map = layers.getCostmap();
// Verify that the circumscribed cost lower bound is as expected: based on the cost function.
//unsigned char c = ilayer->computeCost(8.0);
//ASSERT_EQ(ilayer->getCircumscribedCost(), c);
for(unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++){
// To the right
ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// To the left
ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// Down
ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// Up
ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
}
// Verify the normalized cost attenuates as expected
for(unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++){
unsigned char expectedValue = ilayer->computeCost(i/1.0);
ASSERT_EQ(map->getCost(50 + i, 50), expectedValue);
}
// Update with no hits. Should clear (revert to the static map
/*map->resetMapOutsideWindow(0, 0, 0.0, 0.0);
cloud.points.resize(0);
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
Observation obs2(p, cloud, 100.0, 100.0);
std::vector<Observation> obsBuf2;
obsBuf2.push_back(obs2);
map->updateWorld(0, 0, obsBuf2, obsBuf2);
for(unsigned int i = 0; i < 100; i++)
for(unsigned int j = 0; j < 100; j++)
ASSERT_EQ(map->getCost(i, j), costmap_2d::FREE_SPACE);*/
}
