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);*/ }