inflation_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 
00034 #include <map>
00035 
00036 #include <costmap_2d/costmap_2d.h>
00037 #include <costmap_2d/layered_costmap.h>
00038 #include <costmap_2d/obstacle_layer.h>
00039 #include <costmap_2d/inflation_layer.h>
00040 #include <costmap_2d/observation_buffer.h>
00041 #include <costmap_2d/testing_helper.h>
00042 #include <gtest/gtest.h>
00043 #include <tf/transform_listener.h>
00044 
00045 using namespace costmap_2d;
00046 using geometry_msgs::Point;
00047 
00048 std::vector<Point> setRadii(LayeredCostmap& layers, double length, double width, double inflation_radius)
00049 {
00050   std::vector<Point> polygon;
00051   Point p;
00052   p.x = width;
00053   p.y = length; 
00054   polygon.push_back(p);
00055   p.x = width;
00056   p.y = -length; 
00057   polygon.push_back(p);
00058   p.x = -width;
00059   p.y = -length; 
00060   polygon.push_back(p);
00061   p.x = -width;
00062   p.y = length; 
00063   polygon.push_back(p);
00064   layers.setFootprint(polygon);
00065 
00066   ros::NodeHandle nh;
00067   nh.setParam("/inflation_tests/inflation/inflation_radius", inflation_radius);
00068 
00069   return polygon;
00070 }
00071 
00072 // Test that a single point gets inflated properly
00073 void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap, InflationLayer* ilayer, double inflation_radius)
00074 {
00075   bool* seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
00076   memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
00077   std::map<double, std::vector<CellData> > m;
00078   CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
00079   m[0].push_back(initial);
00080   for (std::map<double, std::vector<CellData> >::iterator bin = m.begin(); bin != m.end(); ++bin)
00081   {
00082     for (int i = 0; i < bin->second.size(); ++i)
00083     {
00084       const CellData& cell = bin->second[i];
00085       if (!seen[cell.index_])
00086       {
00087         seen[cell.index_] = true;
00088         unsigned int dx = abs(cell.x_ - cell.src_x_);
00089         unsigned int dy = abs(cell.y_ - cell.src_y_);
00090         double dist = hypot(dx, dy);
00091 
00092         unsigned char expected_cost = ilayer->computeCost(dist);
00093         ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);
00094 
00095         if (dist > inflation_radius)
00096         {
00097           continue;
00098         }
00099 
00100         if (cell.x_ > 0)
00101         {
00102           CellData data(costmap->getIndex(cell.x_-1, cell.y_),
00103                         cell.x_-1, cell.y_, cell.src_x_, cell.src_y_);
00104           m[dist].push_back(data);
00105         }
00106         if (cell.y_ > 0)
00107         {
00108           CellData data(costmap->getIndex(cell.x_, cell.y_-1),
00109                         cell.x_, cell.y_-1, cell.src_x_, cell.src_y_);
00110           m[dist].push_back(data);
00111         }
00112         if (cell.x_ < costmap->getSizeInCellsX() - 1)
00113         {
00114           CellData data(costmap->getIndex(cell.x_+1, cell.y_),
00115                         cell.x_+1, cell.y_, cell.src_x_, cell.src_y_);
00116           m[dist].push_back(data);
00117         }
00118         if (cell.y_ < costmap->getSizeInCellsY() - 1)
00119         {
00120           CellData data(costmap->getIndex(cell.x_, cell.y_+1),
00121                         cell.x_, cell.y_+1, cell.src_x_, cell.src_y_);
00122           m[dist].push_back(data);
00123         }
00124       }
00125     }
00126   }
00127   delete[] seen;
00128 }
00129 
00130 TEST(costmap, testAdjacentToObstacleCanStillMove){
00131   tf::TransformListener tf;
00132   LayeredCostmap layers("frame", false, false);
00133   layers.resizeMap(10, 10, 1, 0, 0);
00134 
00135   // Footprint with inscribed radius = 2.1
00136   //               circumscribed radius = 3.1
00137   std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
00138 
00139   ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
00140   InflationLayer* ilayer = addInflationLayer(layers, tf);
00141   layers.setFootprint(polygon);
00142 
00143   addObservation(olayer, 0, 0, MAX_Z);
00144 
00145   layers.updateMap(0,0,0);
00146   Costmap2D* costmap = layers.getCostmap();
00147   //printMap(*costmap);
00148   EXPECT_EQ( LETHAL_OBSTACLE, costmap->getCost( 0, 0 ));
00149   EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 0 ));
00150   EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 2, 0 ));
00151   EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 3, 0 ));
00152   EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 2, 1 ));
00153   EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 1 ));
00154 }
00155 
00156 TEST(costmap, testInflationShouldNotCreateUnknowns){
00157   tf::TransformListener tf;
00158   LayeredCostmap layers("frame", false, false);
00159   layers.resizeMap(10, 10, 1, 0, 0);
00160 
00161   // Footprint with inscribed radius = 2.1
00162   //               circumscribed radius = 3.1
00163   std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
00164 
00165   ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
00166   InflationLayer* ilayer = addInflationLayer(layers, tf);
00167   layers.setFootprint(polygon);
00168 
00169   addObservation(olayer, 0, 0, MAX_Z);
00170 
00171   layers.updateMap(0,0,0);
00172   Costmap2D* costmap = layers.getCostmap();
00173 
00174   EXPECT_EQ( countValues(*costmap, NO_INFORMATION), 0 );
00175 }
00176 
00177 
00181 TEST(costmap, testCostFunctionCorrectness){
00182   tf::TransformListener tf;
00183   LayeredCostmap layers("frame", false, false);
00184   layers.resizeMap(100, 100, 1, 0, 0);
00185 
00186   // Footprint with inscribed radius = 5.0
00187   //               circumscribed radius = 8.0
00188   std::vector<Point> polygon = setRadii(layers, 5.0, 6.25, 10.5);
00189 
00190   ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
00191   InflationLayer* ilayer = addInflationLayer(layers, tf);
00192   layers.setFootprint(polygon);
00193 
00194   addObservation(olayer, 50, 50, MAX_Z);
00195 
00196   layers.updateMap(0,0,0);
00197   Costmap2D* map = layers.getCostmap();
00198 
00199   // Verify that the circumscribed cost lower bound is as expected: based on the cost function.
00200   //unsigned char c = ilayer->computeCost(8.0);
00201   //ASSERT_EQ(ilayer->getCircumscribedCost(), c);
00202 
00203   for(unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++){
00204     // To the right
00205     ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00206     ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00207     // To the left
00208     ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00209     ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00210     // Down
00211     ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00212     ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00213     // Up
00214     ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00215     ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00216   }
00217 
00218   // Verify the normalized cost attenuates as expected
00219   for(unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++){
00220     unsigned char expectedValue = ilayer->computeCost(i/1.0);
00221     ASSERT_EQ(map->getCost(50 + i, 50), expectedValue);
00222   }
00223 
00224   // Update with no hits. Should clear (revert to the static map
00225   /*map->resetMapOutsideWindow(0, 0, 0.0, 0.0);
00226   cloud.points.resize(0);
00227 
00228   p.x = 0.0;
00229   p.y = 0.0;
00230   p.z = MAX_Z;
00231 
00232   Observation obs2(p, cloud, 100.0, 100.0);
00233   std::vector<Observation> obsBuf2;
00234   obsBuf2.push_back(obs2);
00235 
00236   map->updateWorld(0, 0, obsBuf2, obsBuf2);
00237 
00238   for(unsigned int i = 0; i < 100; i++)
00239     for(unsigned int j = 0; j < 100; j++)
00240       ASSERT_EQ(map->getCost(i, j), costmap_2d::FREE_SPACE);*/
00241 }
00242 
00249 TEST(costmap, testInflationOrderCorrectness){
00250   tf::TransformListener tf;
00251   LayeredCostmap layers("frame", false, false);
00252   layers.resizeMap(10, 10, 1, 0, 0);
00253 
00254   // Footprint with inscribed radius = 2.1
00255   //               circumscribed radius = 3.1
00256   const double inflation_radius = 4.1;
00257   std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, inflation_radius);
00258 
00259   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00260   InflationLayer* ilayer = addInflationLayer(layers, tf);
00261   layers.setFootprint(polygon);
00262 
00263   // Add two diagonal cells, they would induce problems under the
00264   // previous implementations
00265   addObservation(olayer, 4, 4, MAX_Z);
00266   addObservation(olayer, 5, 5, MAX_Z);
00267 
00268   layers.updateMap(0, 0, 0);
00269 
00270   validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
00271   validatePointInflation(5, 5, layers.getCostmap(), ilayer, inflation_radius);
00272 }
00273 
00277 TEST(costmap, testInflation){
00278 
00279   tf::TransformListener tf;
00280   LayeredCostmap layers("frame", false, false);
00281 
00282   // Footprint with inscribed radius = 2.1
00283   //               circumscribed radius = 3.1
00284   std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
00285 
00286   addStaticLayer(layers, tf);
00287   ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
00288   InflationLayer* ilayer = addInflationLayer(layers, tf);
00289   layers.setFootprint(polygon);
00290 
00291   Costmap2D* costmap = layers.getCostmap();
00292 
00293   layers.updateMap(0,0,0);
00294   //printMap(*costmap);
00295   ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE),             (unsigned int)20);
00296   ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)28);
00297 
00298   /*/ Iterate over all id's and verify they are obstacles
00299   for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
00300     unsigned int ind = *it;
00301     unsigned int x, y;
00302     map.indexToCells(ind, x, y);
00303     ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
00304     ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00305   }*/
00306 
00307   addObservation(olayer, 0, 0, 0.4);
00308   layers.updateMap(0,0,0);
00309 
00310   // It and its 2 neighbors makes 3 obstacles
00311   ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51);
00312 
00313   // @todo Rewrite 
00314   // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
00315   addObservation(olayer, 2, 0);
00316   layers.updateMap(0,0,0);
00317 
00318   // Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from
00319   // the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle
00320   // at <0, 1>
00321   ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)54);
00322 
00323   // Add an obstacle at <1, 9>. This will inflate obstacles around it
00324   addObservation(olayer, 1, 9);
00325   layers.updateMap(0,0,0);
00326 
00327   ASSERT_EQ(costmap->getCost(1, 9), LETHAL_OBSTACLE);
00328   ASSERT_EQ(costmap->getCost(0, 9), INSCRIBED_INFLATED_OBSTACLE);
00329   ASSERT_EQ(costmap->getCost(2, 9), INSCRIBED_INFLATED_OBSTACLE);
00330 
00331   // Add an obstacle and verify that it over-writes its inflated status
00332   addObservation(olayer, 0, 9);
00333   layers.updateMap(0,0,0);
00334 
00335   ASSERT_EQ(costmap->getCost(0, 9), LETHAL_OBSTACLE);
00336 }
00337 
00341 TEST(costmap, testInflation2){
00342 
00343   tf::TransformListener tf;
00344   LayeredCostmap layers("frame", false, false);
00345 
00346   // Footprint with inscribed radius = 2.1
00347   //               circumscribed radius = 3.1
00348   std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
00349 
00350   addStaticLayer(layers, tf);
00351   ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
00352   InflationLayer* ilayer = addInflationLayer(layers, tf);
00353   layers.setFootprint(polygon);
00354 
00355   // Creat a small L-Shape all at once
00356   addObservation(olayer, 1, 1, MAX_Z);
00357   addObservation(olayer, 2, 1, MAX_Z);
00358   addObservation(olayer, 2, 2, MAX_Z);
00359   layers.updateMap(0,0,0);
00360 
00361   Costmap2D* costmap = layers.getCostmap();
00362   //printMap(*costmap);
00363   ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);  
00364   ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
00365 }
00366 
00370 TEST(costmap, testInflation3){
00371   tf::TransformListener tf;
00372   LayeredCostmap layers("frame", false, false);
00373   layers.resizeMap(10, 10, 1, 0, 0);
00374 
00375   // 1 2 3
00376   std::vector<Point> polygon = setRadii(layers, 1, 1.75, 3);
00377 
00378   ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
00379   InflationLayer* ilayer = addInflationLayer(layers, tf);
00380   layers.setFootprint(polygon);
00381 
00382   // There should be no occupied cells
00383   Costmap2D* costmap = layers.getCostmap();
00384   ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE),             (unsigned int)0);
00385   ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)0);
00386   printMap(*costmap);
00387   // Add an obstacle at 5,5
00388   addObservation(olayer, 5, 5, MAX_Z);
00389   layers.updateMap(0,0,0);
00390   printMap(*costmap);
00391 
00392   // Test fails because updated cell value is 0
00393   ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29);
00394   ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1);
00395   ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4);
00396 
00397   // Update again - should see no change
00398   layers.updateMap(0,0,0);
00399 
00400   ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29);
00401   ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1);
00402   ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4);
00403 }
00404 
00405 
00406 int main(int argc, char** argv){
00407   ros::init(argc, argv, "inflation_tests");
00408   testing::InitGoogleTest(&argc, argv);
00409   return RUN_ALL_TESTS();
00410 }


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15