obstacle_tests.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00035 #include <costmap_2d/costmap_2d.h>
00036 #include <costmap_2d/layered_costmap.h>
00037 #include <costmap_2d/observation_buffer.h>
00038 #include <costmap_2d/testing_helper.h>
00039 #include <set>
00040 #include <gtest/gtest.h>
00041 #include <tf/transform_listener.h>
00042 
00043 using namespace costmap_2d;
00044 
00045 /*
00046  * For reference, the static map looks like this:
00047  *
00048  *   0   0   0   0   0   0   0 254 254 254
00049  *
00050  *   0   0   0   0   0   0   0 254 254 254
00051  *
00052  *   0   0   0 254 254 254   0   0   0   0
00053  *
00054  *   0   0   0   0   0   0   0   0   0   0
00055  *
00056  *   0   0   0   0   0   0   0   0   0   0
00057  *
00058  *   0   0   0   0 254   0   0 254 254 254
00059  *
00060  *   0   0   0   0 254   0   0 254 254 254
00061  *
00062  *   0   0   0   0   0   0   0 254 254 254
00063  *
00064  *   0   0   0   0   0   0   0   0   0   0
00065  *
00066  *   0   0   0   0   0   0   0   0   0   0
00067  *
00068  *   upper left is 0,0, lower right is 9,9
00069  */
00070 
00074 TEST(costmap, testRaytracing){
00075   tf::TransformListener tf;
00076 
00077   LayeredCostmap layers("frame", false, false);  // Not rolling window, not tracking unknown
00078   addStaticLayer(layers, tf);  // This adds the static map
00079   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00080   
00081   // Add a point at 0, 0, 0
00082   addObservation(olayer, 0.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
00083 
00084   // This actually puts the LETHAL (254) point in the costmap at (0,0)
00085   layers.updateMap(0,0,0);  // 0, 0, 0 is robot pose
00086   //printMap(*(layers.getCostmap()));
00087   
00088   int lethal_count = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
00089 
00090   // We expect just one obstacle to be added (20 in static map)
00091   ASSERT_EQ(lethal_count, 21);
00092 }
00093 
00097 TEST(costmap, testRaytracing2){
00098   tf::TransformListener tf;
00099   LayeredCostmap layers("frame", false, false);
00100   addStaticLayer(layers, tf);
00101   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00102 
00103   //If we print map now, it is 10x10 all value 0
00104   //printMap(*(layers.getCostmap()));
00105 
00106   // Update will fill in the costmap with the static map
00107   layers.updateMap(0,0,0);
00108 
00109   //If we print the map now, we get the static map
00110   //printMap(*(layers.getCostmap()));
00111 
00112   // Static map has 20 LETHAL cells (see diagram above)
00113   int obs_before = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
00114   ASSERT_EQ(obs_before, 20);
00115 
00116   // The sensor origin will be <0,0>. So if we add an obstacle at 9,9,
00117   // we would expect cells <0, 0> thru <8, 8> to be traced through
00118   // however the static map is not cleared by obstacle layer
00119   addObservation(olayer, 9.5, 9.5, MAX_Z/2, 0.5, 0.5, MAX_Z/2);
00120   layers.updateMap(0, 0, 0);
00121 
00122   // If we print map now, we have static map + <9,9> is LETHAL
00123   //printMap(*(layers.getCostmap()));
00124   int obs_after = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
00125 
00126   // Change from previous test:
00127   // No obstacles from the static map will be cleared, so the
00128   // net change is +1. 
00129   ASSERT_EQ(obs_after, obs_before + 1);
00130 
00131   // Fill in the diagonal, <7,7> and <9,9> already filled in, <0,0> is robot
00132   for(int i = 0; i < olayer->getSizeInCellsY(); ++i)
00133   {
00134     olayer->setCost(i, i, LETHAL_OBSTACLE);
00135   }
00136   // This will updateBounds, which will raytrace the static observation added
00137   // above, thus clearing out the diagonal again!
00138   layers.updateMap(0, 0, 0);
00139 
00140   // Map now has diagonal except <0,0> filled with LETHAL (254)
00141   //printMap(*(layers.getCostmap()));
00142   int with_static = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
00143 
00144   // Should thus be the same
00145   ASSERT_EQ(with_static, obs_after);
00146   // If 21 are filled, 79 should be free
00147   ASSERT_EQ(79, countValues(*(layers.getCostmap()), costmap_2d::FREE_SPACE));
00148 }
00149 
00153 TEST(costmap, testWaveInterference){
00154   tf::TransformListener tf;
00155 
00156   // Start with an empty map, no rolling window, tracking unknown
00157   LayeredCostmap layers("frame", false, true);
00158   layers.resizeMap(10, 10, 1, 0, 0);
00159   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00160 
00161   // If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION)
00162   //printMap(*(layers.getCostmap()));
00163 
00164   // Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
00165   addObservation(olayer, 3.0, 3.0, MAX_Z);
00166   addObservation(olayer, 5.0, 5.0, MAX_Z);
00167   addObservation(olayer, 7.0, 7.0, MAX_Z);
00168   layers.updateMap(0,0,0);
00169 
00170   Costmap2D* costmap = layers.getCostmap();
00171   // 3 obstacle cells are filled, <1,1>,<2,2>,<4,4> and <6,6> are now free
00172   // <0,0> is footprint and is free
00173   //printMap(*costmap);
00174   ASSERT_EQ(3, countValues(*costmap, costmap_2d::LETHAL_OBSTACLE));
00175   ASSERT_EQ(92, countValues(*costmap, costmap_2d::NO_INFORMATION));
00176   ASSERT_EQ(5, countValues(*costmap, costmap_2d::FREE_SPACE));
00177 }
00178 
00182 TEST(costmap, testZThreshold){
00183   tf::TransformListener tf;
00184   // Start with an empty map
00185   LayeredCostmap layers("frame", false, true);
00186   layers.resizeMap(10, 10, 1, 0, 0);
00187 
00188   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00189 
00190   // A point cloud with 2 points falling in a cell with a non-lethal cost
00191   addObservation(olayer, 0.0, 5.0, 0.4);
00192   addObservation(olayer, 1.0, 5.0, 2.2);
00193 
00194   layers.updateMap(0,0,0);
00195 
00196   Costmap2D* costmap = layers.getCostmap();
00197   ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 1);
00198 }
00199 
00200 
00204 TEST(costmap, testDynamicObstacles){
00205   tf::TransformListener tf;
00206   LayeredCostmap layers("frame", false, false);
00207   addStaticLayer(layers, tf);
00208 
00209   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00210 
00211   // Add a point cloud and verify its insertion. There should be only one new one
00212   addObservation(olayer, 0.0, 0.0);
00213   addObservation(olayer, 0.0, 0.0);
00214   addObservation(olayer, 0.0, 0.0);
00215 
00216   layers.updateMap(0,0,0);
00217 
00218   Costmap2D* costmap = layers.getCostmap();
00219   // Should now have 1 insertion and no deletions
00220   ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
00221 
00222   // Repeating the call - we should see no insertions or deletions
00223   ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
00224 }
00225 
00226 
00230 TEST(costmap, testMultipleAdditions){
00231   tf::TransformListener tf;
00232   LayeredCostmap layers("frame", false, false);
00233   addStaticLayer(layers, tf);
00234 
00235   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00236 
00237   // A point cloud with one point that falls within an existing obstacle
00238   addObservation(olayer, 9.5, 0.0);
00239   layers.updateMap(0,0,0);
00240   Costmap2D* costmap = layers.getCostmap();
00241   //printMap(*costmap);
00242 
00243   ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 20);
00244 
00245 }
00246 
00247 
00248 int main(int argc, char** argv){
00249   ros::init(argc, argv, "obstacle_tests");
00250   testing::InitGoogleTest(&argc, argv);
00251   return RUN_ALL_TESTS();
00252 }


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:21