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