252 lines
8.1 KiB
C++
Executable File
252 lines
8.1 KiB
C++
Executable File
/*
|
|
* Copyright (c) 2013, Willow Garage, Inc.
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
* * Neither the name of the Willow Garage, Inc. nor the names of its
|
|
* contributors may be used to endorse or promote products derived from
|
|
* this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
|
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
/**
|
|
* @author David Lu!!
|
|
* Test harness for ObstacleLayer for Costmap2D
|
|
*/
|
|
|
|
#include <costmap_2d/costmap_2d.h>
|
|
#include <costmap_2d/layered_costmap.h>
|
|
#include <costmap_2d/observation_buffer.h>
|
|
#include <costmap_2d/testing_helper.h>
|
|
#include <set>
|
|
#include <gtest/gtest.h>
|
|
|
|
using namespace costmap_2d;
|
|
|
|
/*
|
|
* For reference, the static map looks like this:
|
|
*
|
|
* 0 0 0 0 0 0 0 254 254 254
|
|
*
|
|
* 0 0 0 0 0 0 0 254 254 254
|
|
*
|
|
* 0 0 0 254 254 254 0 0 0 0
|
|
*
|
|
* 0 0 0 0 0 0 0 0 0 0
|
|
*
|
|
* 0 0 0 0 0 0 0 0 0 0
|
|
*
|
|
* 0 0 0 0 254 0 0 254 254 254
|
|
*
|
|
* 0 0 0 0 254 0 0 254 254 254
|
|
*
|
|
* 0 0 0 0 0 0 0 254 254 254
|
|
*
|
|
* 0 0 0 0 0 0 0 0 0 0
|
|
*
|
|
* 0 0 0 0 0 0 0 0 0 0
|
|
*
|
|
* upper left is 0,0, lower right is 9,9
|
|
*/
|
|
|
|
/**
|
|
* Test for ray tracing free space
|
|
*/
|
|
TEST(costmap, testRaytracing){
|
|
tf2_ros::Buffer tf;
|
|
|
|
LayeredCostmap layers("frame", false, false); // Not rolling window, not tracking unknown
|
|
addStaticLayer(layers, tf); // This adds the static map
|
|
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
|
|
|
|
// Add a point at 0, 0, 0
|
|
addObservation(olayer, 0.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
|
|
|
|
// This actually puts the LETHAL (254) point in the costmap at (0,0)
|
|
layers.updateMap(0,0,0); // 0, 0, 0 is robot pose
|
|
//printMap(*(layers.getCostmap()));
|
|
|
|
int lethal_count = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
|
|
|
|
// We expect just one obstacle to be added (20 in static map)
|
|
ASSERT_EQ(lethal_count, 21);
|
|
}
|
|
|
|
/**
|
|
* Test for ray tracing free space
|
|
*/
|
|
TEST(costmap, testRaytracing2){
|
|
tf2_ros::Buffer tf;
|
|
LayeredCostmap layers("frame", false, false);
|
|
addStaticLayer(layers, tf);
|
|
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
|
|
|
|
//If we print map now, it is 10x10 all value 0
|
|
//printMap(*(layers.getCostmap()));
|
|
|
|
// Update will fill in the costmap with the static map
|
|
layers.updateMap(0,0,0);
|
|
|
|
//If we print the map now, we get the static map
|
|
//printMap(*(layers.getCostmap()));
|
|
|
|
// Static map has 20 LETHAL cells (see diagram above)
|
|
int obs_before = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
|
|
ASSERT_EQ(obs_before, 20);
|
|
|
|
// The sensor origin will be <0,0>. So if we add an obstacle at 9,9,
|
|
// we would expect cells <0, 0> thru <8, 8> to be traced through
|
|
// however the static map is not cleared by obstacle layer
|
|
addObservation(olayer, 9.5, 9.5, MAX_Z/2, 0.5, 0.5, MAX_Z/2);
|
|
layers.updateMap(0, 0, 0);
|
|
|
|
// If we print map now, we have static map + <9,9> is LETHAL
|
|
//printMap(*(layers.getCostmap()));
|
|
int obs_after = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
|
|
|
|
// Change from previous test:
|
|
// No obstacles from the static map will be cleared, so the
|
|
// net change is +1.
|
|
ASSERT_EQ(obs_after, obs_before + 1);
|
|
|
|
// Fill in the diagonal, <7,7> and <9,9> already filled in, <0,0> is robot
|
|
for(int i = 0; i < olayer->getSizeInCellsY(); ++i)
|
|
{
|
|
olayer->setCost(i, i, LETHAL_OBSTACLE);
|
|
}
|
|
// This will updateBounds, which will raytrace the static observation added
|
|
// above, thus clearing out the diagonal again!
|
|
layers.updateMap(0, 0, 0);
|
|
|
|
// Map now has diagonal except <0,0> filled with LETHAL (254)
|
|
//printMap(*(layers.getCostmap()));
|
|
int with_static = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
|
|
|
|
// Should thus be the same
|
|
ASSERT_EQ(with_static, obs_after);
|
|
// If 21 are filled, 79 should be free
|
|
ASSERT_EQ(79, countValues(*(layers.getCostmap()), costmap_2d::FREE_SPACE));
|
|
}
|
|
|
|
/**
|
|
* Test for wave interference
|
|
*/
|
|
TEST(costmap, testWaveInterference){
|
|
tf2_ros::Buffer tf;
|
|
|
|
// Start with an empty map, no rolling window, tracking unknown
|
|
LayeredCostmap layers("frame", false, true);
|
|
layers.resizeMap(10, 10, 1, 0, 0);
|
|
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
|
|
|
|
// If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION)
|
|
//printMap(*(layers.getCostmap()));
|
|
|
|
// Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
|
|
addObservation(olayer, 3.0, 3.0, MAX_Z);
|
|
addObservation(olayer, 5.0, 5.0, MAX_Z);
|
|
addObservation(olayer, 7.0, 7.0, MAX_Z);
|
|
layers.updateMap(0,0,0);
|
|
|
|
Costmap2D* costmap = layers.getCostmap();
|
|
// 3 obstacle cells are filled, <1,1>,<2,2>,<4,4> and <6,6> are now free
|
|
// <0,0> is footprint and is free
|
|
//printMap(*costmap);
|
|
ASSERT_EQ(3, countValues(*costmap, costmap_2d::LETHAL_OBSTACLE));
|
|
ASSERT_EQ(92, countValues(*costmap, costmap_2d::NO_INFORMATION));
|
|
ASSERT_EQ(5, countValues(*costmap, costmap_2d::FREE_SPACE));
|
|
}
|
|
|
|
/**
|
|
* Make sure we ignore points outside of our z threshold
|
|
*/
|
|
TEST(costmap, testZThreshold){
|
|
tf2_ros::Buffer tf;
|
|
// Start with an empty map
|
|
LayeredCostmap layers("frame", false, true);
|
|
layers.resizeMap(10, 10, 1, 0, 0);
|
|
|
|
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
|
|
|
|
// A point cloud with 2 points falling in a cell with a non-lethal cost
|
|
addObservation(olayer, 0.0, 5.0, 0.4);
|
|
addObservation(olayer, 1.0, 5.0, 2.2);
|
|
|
|
layers.updateMap(0,0,0);
|
|
|
|
Costmap2D* costmap = layers.getCostmap();
|
|
ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 1);
|
|
}
|
|
|
|
|
|
/**
|
|
* Verify that dynamic obstacles are added
|
|
*/
|
|
TEST(costmap, testDynamicObstacles){
|
|
tf2_ros::Buffer tf;
|
|
LayeredCostmap layers("frame", false, false);
|
|
addStaticLayer(layers, tf);
|
|
|
|
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
|
|
|
|
// Add a point cloud and verify its insertion. There should be only one new one
|
|
addObservation(olayer, 0.0, 0.0);
|
|
addObservation(olayer, 0.0, 0.0);
|
|
addObservation(olayer, 0.0, 0.0);
|
|
|
|
layers.updateMap(0,0,0);
|
|
|
|
Costmap2D* costmap = layers.getCostmap();
|
|
// Should now have 1 insertion and no deletions
|
|
ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
|
|
|
|
// Repeating the call - we should see no insertions or deletions
|
|
ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
|
|
}
|
|
|
|
|
|
/**
|
|
* Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle
|
|
*/
|
|
TEST(costmap, testMultipleAdditions){
|
|
tf2_ros::Buffer tf;
|
|
LayeredCostmap layers("frame", false, false);
|
|
addStaticLayer(layers, tf);
|
|
|
|
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
|
|
|
|
// A point cloud with one point that falls within an existing obstacle
|
|
addObservation(olayer, 9.5, 0.0);
|
|
layers.updateMap(0,0,0);
|
|
Costmap2D* costmap = layers.getCostmap();
|
|
//printMap(*costmap);
|
|
|
|
ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 20);
|
|
|
|
}
|
|
|
|
|
|
int main(int argc, char** argv){
|
|
ros::init(argc, argv, "obstacle_tests");
|
|
testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|