#include <costmap_2d/costmap_2d.h>#include <costmap_2d/layered_costmap.h>#include <costmap_2d/obstacle_layer.h>#include <costmap_2d/static_layer.h>#include <costmap_2d/observation_buffer.h>#include <costmap_2d/testing_helper.h>#include <set>#include <gtest/gtest.h>#include <tf/transform_listener.h>
Go to the source code of this file.
Functions | |
| int | main (int argc, char **argv) |
Function Documentation
| int main | ( | int | argc, |
| char ** | argv | ||
| ) |
Tests the reset method
TEST(costmap, testResetForStaticMap){ Define a static map with a large object in the center std::vector<unsigned char> staticMap; for(unsigned int i=0; i<10; i++){ for(unsigned int j=0; j<10; j++){ staticMap.push_back(costmap_2d::LETHAL_OBSTACLE); } }
Allocate the cost map, with a inflation to 3 cells all around Costmap2D map(10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3, OBSTACLE_RANGE, MAX_Z, RAYTRACE_RANGE, 25, staticMap, THRESHOLD);
Populate the cost map with a wall around the perimeter. Free space should clear out the room. pcl::PointCloud<pcl::PointXYZ> cloud; cloud.points.resize(40);
Left wall unsigned int ind = 0; for (unsigned int i = 0; i < 10; i++){ Left cloud.points[ind].x = 0; cloud.points[ind].y = i; cloud.points[ind].z = MAX_Z; ind++;
Top cloud.points[ind].x = i; cloud.points[ind].y = 0; cloud.points[ind].z = MAX_Z; ind++;
Right cloud.points[ind].x = 9; cloud.points[ind].y = i; cloud.points[ind].z = MAX_Z; ind++;
Bottom cloud.points[ind].x = i; cloud.points[ind].y = 9; cloud.points[ind].z = MAX_Z; ind++; }
double wx = 5.0, wy = 5.0; geometry_msgs::Point p; p.x = wx; p.y = wy; p.z = MAX_Z; Observation obs(p, cloud, OBSTACLE_RANGE, RAYTRACE_RANGE); std::vector<Observation> obsBuf; obsBuf.push_back(obs);
Update the cost map for this observation map.updateWorld(wx, wy, obsBuf, obsBuf);
Verify that we now have only 36 cells with lethal cost, thus provong that we have correctly cleared free space int hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ hitCount++; } } } ASSERT_EQ(hitCount, 36);
Veriy that we have 64 non-leathal hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ if(map.getCost(i, j) != costmap_2d::LETHAL_OBSTACLE) hitCount++; } } ASSERT_EQ(hitCount, 64);
Now if we reset the cost map, we should have our map go back to being completely occupied map.resetMapOutsideWindow(wx, wy, 0.0, 0.0);
We should now go back to everything being occupied hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE) hitCount++; } } ASSERT_EQ(hitCount, 100);
}
/** 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 updating costmaps with static data TEST(costmap, testFullyContainedStaticMapUpdate){ Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD);
Costmap2D static_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);
map.updateStaticMapWindow(0, 0, 10, 10, MAP_10_BY_10);
for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){ for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){ ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j)); } } }
TEST(costmap, testOverlapStaticMapUpdate){ Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD);
Costmap2D static_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);
map.updateStaticMapWindow(-10, -10, 10, 10, MAP_10_BY_10);
ASSERT_FLOAT_EQ(map.getOriginX(), -10); ASSERT_FLOAT_EQ(map.getOriginX(), -10); ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15); ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15); for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j)); } }
std::vector<unsigned char> blank(100);
check to make sure that inflation on updates are being done correctly map.updateStaticMapWindow(-10, -10, 10, 10, blank);
for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){ for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){ ASSERT_EQ(map.getCost(i, j), 0); } }
std::vector<unsigned char> fully_contained(25); fully_contained[0] = 254; fully_contained[4] = 254; fully_contained[5] = 254; fully_contained[9] = 254;
Costmap2D small_static_map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 10.0, MAX_Z, 10.0, 25, fully_contained, THRESHOLD);
map.updateStaticMapWindow(0, 0, 5, 5, fully_contained);
ASSERT_FLOAT_EQ(map.getOriginX(), -10); ASSERT_FLOAT_EQ(map.getOriginX(), -10); ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15); ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15); for(unsigned int j = 0; j < 5; ++j){ for(unsigned int i = 0; i < 5; ++i){ ASSERT_EQ(map.getCost(i + 10, j + 10), small_static_map.getCost(i, j)); } }
}
TEST(costmap, testStaticMap){ 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);
ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10); ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
Verify that obstacles correctly identified from the static map. std::vector<unsigned int> occupiedCells;
for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ occupiedCells.push_back(map.getIndex(i, j)); } } }
ASSERT_EQ(occupiedCells.size(), (unsigned int)20);
Iterate over all id's and verify that they are present according to their 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_10_BY_10[ind] >= 100, true); ASSERT_EQ(map.getCost(x, y) >= 100, true); }
Block of 200 ASSERT_EQ(find(occupiedCells, map.getIndex(7, 2)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(8, 2)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(9, 2)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(7, 3)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(8, 3)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(9, 3)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(7, 4)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(8, 4)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(9, 4)), true);
Block of 100 ASSERT_EQ(find(occupiedCells, map.getIndex(4, 3)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(4, 4)), true);
Block of 200 ASSERT_EQ(find(occupiedCells, map.getIndex(3, 7)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(4, 7)), true); ASSERT_EQ(find(occupiedCells, map.getIndex(5, 7)), true);
Verify Coordinate Transformations, ROW MAJOR ORDER ASSERT_EQ(worldToIndex(map, 0.0, 0.0), (unsigned int)0); ASSERT_EQ(worldToIndex(map, 0.0, 0.99), (unsigned int)0); ASSERT_EQ(worldToIndex(map, 0.0, 1.0), (unsigned int)10); ASSERT_EQ(worldToIndex(map, 1.0, 0.99), (unsigned int)1); ASSERT_EQ(worldToIndex(map, 9.99, 9.99), (unsigned int)99); ASSERT_EQ(worldToIndex(map, 8.2, 3.4), (unsigned int)38);
Ensure we hit the middle of the cell for world co-ordinates double wx, wy; indexToWorld(map, 99, wx, wy); ASSERT_EQ(wx, 9.5); ASSERT_EQ(wy, 9.5); }
Definition at line 327 of file static_tests.cpp.