module_tests.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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/observation_buffer.h>
00037 #include <set>
00038 #include <gtest/gtest.h>
00039 #include <tf/transform_listener.h>
00040 
00041 using namespace costmap_2d;
00042 
00043 const unsigned char MAP_10_BY_10_CHAR[] = {
00044   0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00045   0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00046   0, 0, 0, 0, 0, 0, 0, 200, 200, 200,
00047   0, 0, 0, 0, 100, 0, 0, 200, 200, 200,
00048   0, 0, 0, 0, 100, 0, 0, 200, 200, 200,
00049   70, 70, 0, 0, 0, 0, 0, 0, 0, 0,
00050   0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00051   0, 0, 0, 200, 200, 200, 0, 0, 0, 0,
00052   0, 0, 0, 0, 0, 0, 0, 255, 255, 255,
00053   0, 0, 0, 0, 0, 0, 0, 255, 255, 255
00054 };
00055 
00056 const unsigned char MAP_5_BY_5_CHAR[] = {
00057   0, 0, 0, 0, 0,
00058   0, 0, 0, 0, 0,
00059   0, 0, 0, 0, 0,
00060   0, 0, 0, 0, 0,
00061   0, 0, 0, 0, 0,
00062 };
00063 
00064 std::vector<unsigned char> MAP_5_BY_5;
00065 std::vector<unsigned char> MAP_10_BY_10;
00066 std::vector<unsigned char> EMPTY_10_BY_10;
00067 std::vector<unsigned char> EMPTY_100_BY_100;
00068 
00069 const unsigned int GRID_WIDTH(10);
00070 const unsigned int GRID_HEIGHT(10);
00071 const double RESOLUTION(1);
00072 const double WINDOW_LENGTH(10);
00073 const unsigned char THRESHOLD(100);
00074 const double MAX_Z(1.0);
00075 const double RAYTRACE_RANGE(20.0);
00076 const double OBSTACLE_RANGE(20.0);
00077 const double ROBOT_RADIUS(1.0);
00078 
00079 bool find(const std::vector<unsigned int>& l, unsigned int n){
00080   for(std::vector<unsigned int>::const_iterator it = l.begin(); it != l.end(); ++it){
00081     if(*it == n)
00082       return true;
00083   }
00084 
00085   return false;
00086 }
00087 
00091 TEST(costmap, testResetForStaticMap){
00092   // Define a static map with a large object in the center
00093   std::vector<unsigned char> staticMap;
00094   for(unsigned int i=0; i<10; i++){
00095     for(unsigned int j=0; j<10; j++){
00096       staticMap.push_back(costmap_2d::LETHAL_OBSTACLE);
00097     }
00098   }
00099 
00100   // Allocate the cost map, with a inflation to 3 cells all around
00101   Costmap2D map(10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3, OBSTACLE_RANGE, MAX_Z, RAYTRACE_RANGE, 25, staticMap, THRESHOLD);
00102 
00103   // Populate the cost map with a wall around the perimeter. Free space should clear out the room.
00104   pcl::PointCloud<pcl::PointXYZ> cloud;
00105   cloud.points.resize(40);
00106 
00107   // Left wall
00108   unsigned int ind = 0;
00109   for (unsigned int i = 0; i < 10; i++){
00110     // Left
00111     cloud.points[ind].x = 0;
00112     cloud.points[ind].y = i;
00113     cloud.points[ind].z = MAX_Z;
00114     ind++;
00115 
00116     // Top
00117     cloud.points[ind].x = i;
00118     cloud.points[ind].y = 0;
00119     cloud.points[ind].z = MAX_Z;
00120     ind++;
00121 
00122     // Right
00123     cloud.points[ind].x = 9;
00124     cloud.points[ind].y = i;
00125     cloud.points[ind].z = MAX_Z;
00126     ind++;
00127 
00128     // Bottom
00129     cloud.points[ind].x = i;
00130     cloud.points[ind].y = 9;
00131     cloud.points[ind].z = MAX_Z;
00132     ind++;
00133   }
00134 
00135   double wx = 5.0, wy = 5.0;
00136   geometry_msgs::Point p;
00137   p.x = wx;
00138   p.y = wy;
00139   p.z = MAX_Z;
00140   Observation obs(p, cloud, OBSTACLE_RANGE, RAYTRACE_RANGE);
00141   std::vector<Observation> obsBuf;
00142   obsBuf.push_back(obs);
00143 
00144   // Update the cost map for this observation
00145   map.updateWorld(wx, wy, obsBuf, obsBuf);
00146 
00147   // Verify that we now have only 36 cells with lethal cost, thus provong that we have correctly cleared
00148   // free space
00149   int hitCount = 0;
00150   for(unsigned int i=0; i < 10; ++i){
00151     for(unsigned int j=0; j < 10; ++j){
00152       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
00153         hitCount++;
00154       }
00155     }
00156   }
00157   ASSERT_EQ(hitCount, 36);
00158 
00159   // Veriy that we have 64 non-leathal
00160   hitCount = 0;
00161   for(unsigned int i=0; i < 10; ++i){
00162     for(unsigned int j=0; j < 10; ++j){
00163       if(map.getCost(i, j) != costmap_2d::LETHAL_OBSTACLE)
00164         hitCount++;
00165     }
00166   }
00167   ASSERT_EQ(hitCount, 64);
00168 
00169   // Now if we reset the cost map, we should have our map go back to being completely occupied
00170   map.resetMapOutsideWindow(wx, wy, 0.0, 0.0);
00171 
00172   //We should now go back to everything being occupied
00173   hitCount = 0;
00174   for(unsigned int i=0; i < 10; ++i){
00175     for(unsigned int j=0; j < 10; ++j){
00176       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
00177         hitCount++;
00178     }
00179   }
00180   ASSERT_EQ(hitCount, 100);
00181 
00182 }
00183 
00187 TEST(costmap, testCostFunctionCorrectness){
00188   Costmap2D map(100, 100, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS * 5.0, ROBOT_RADIUS * 8.0, ROBOT_RADIUS * 10.5, 
00189       100.0, MAX_Z, 100.0, 25, EMPTY_100_BY_100, THRESHOLD);
00190 
00191   // Verify that the circumscribed cost lower bound is as expected: based on the cost function.
00192   unsigned char c = map.computeCost((ROBOT_RADIUS * 8.0 / RESOLUTION));
00193   ASSERT_EQ(map.getCircumscribedCost(), c);
00194 
00195   // Add a point in the center
00196   pcl::PointCloud<pcl::PointXYZ> cloud;
00197   cloud.points.resize(1);
00198   cloud.points[0].x = 50;
00199   cloud.points[0].y = 50;
00200   cloud.points[0].z = MAX_Z;
00201 
00202   geometry_msgs::Point p;
00203   p.x = 0.0;
00204   p.y = 0.0;
00205   p.z = MAX_Z;
00206 
00207   Observation obs(p, cloud, 100.0, 100.0);
00208   std::vector<Observation> obsBuf;
00209   obsBuf.push_back(obs);
00210 
00211   map.updateWorld(0, 0, obsBuf, obsBuf);
00212 
00213   for(unsigned int i = 0; i <= (unsigned int)ceil(ROBOT_RADIUS * 5.0); i++){
00214     // To the right
00215     ASSERT_EQ(map.getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00216     ASSERT_EQ(map.getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00217     // To the left
00218     ASSERT_EQ(map.getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00219     ASSERT_EQ(map.getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00220     // Down
00221     ASSERT_EQ(map.getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00222     ASSERT_EQ(map.getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00223     // Up
00224     ASSERT_EQ(map.getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00225     ASSERT_EQ(map.getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00226   }
00227 
00228   // Verify the normalized cost attenuates as expected
00229   for(unsigned int i = (unsigned int)(ceil(ROBOT_RADIUS * 5.0) + 1); i <= (unsigned int)ceil(ROBOT_RADIUS * 10.5); i++){
00230     unsigned char expectedValue = map.computeCost(i / RESOLUTION);
00231     ASSERT_EQ(map.getCost(50 + i, 50), expectedValue);
00232   }
00233 
00234   // Update with no hits. Should clear (revert to the static map
00235   map.resetMapOutsideWindow(0, 0, 0.0, 0.0);
00236   cloud.points.resize(0);
00237 
00238   p.x = 0.0;
00239   p.y = 0.0;
00240   p.z = MAX_Z;
00241 
00242   Observation obs2(p, cloud, 100.0, 100.0);
00243   std::vector<Observation> obsBuf2;
00244   obsBuf2.push_back(obs2);
00245 
00246   map.updateWorld(0, 0, obsBuf2, obsBuf2);
00247 
00248   for(unsigned int i = 0; i < 100; i++)
00249     for(unsigned int j = 0; j < 100; j++)
00250       ASSERT_EQ(map.getCost(i, j), costmap_2d::FREE_SPACE);
00251 }
00252 
00253 char printableCost( unsigned char cost )
00254 {
00255   switch( cost )
00256   {
00257   case NO_INFORMATION: return '?';
00258   case LETHAL_OBSTACLE: return 'L';
00259   case INSCRIBED_INFLATED_OBSTACLE: return 'I';
00260   case FREE_SPACE: return '.';
00261   default: return '0' + (unsigned char) (10 * cost / 255);
00262   }
00263 }
00264 
00268 TEST(costmap, testWaveInterference){
00269   // Start with an empty map
00270   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS * 2, ROBOT_RADIUS * 3.01,
00271       10.0, MAX_Z * 2, 10.0, 1, EMPTY_10_BY_10, THRESHOLD);
00272 
00273   // Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
00274   pcl::PointCloud<pcl::PointXYZ> cloud;
00275   cloud.points.resize(3);
00276   cloud.points[0].x = 3;
00277   cloud.points[0].y = 3;
00278   cloud.points[0].z = MAX_Z;
00279   cloud.points[1].x = 5;
00280   cloud.points[1].y = 5;
00281   cloud.points[1].z = MAX_Z;
00282   cloud.points[2].x = 7;
00283   cloud.points[2].y = 7;
00284   cloud.points[2].z = MAX_Z;
00285 
00286   geometry_msgs::Point p;
00287   p.x = 0.0;
00288   p.y = 0.0;
00289   p.z = MAX_Z;
00290 
00291   Observation obs(p, cloud, 100.0, 100.0);
00292   std::vector<Observation> obsBuf;
00293   obsBuf.push_back(obs);
00294 
00295   map.updateWorld(0, 0, obsBuf, obsBuf);
00296 
00297   int update_count = 0;
00298 
00299   // Expect to see a union of obstacles
00300   printf("map:\n");
00301   for(unsigned int i = 0; i < 10; ++i){
00302     for(unsigned int j = 0; j < 10; ++j){
00303       if(map.getCost(i, j) != costmap_2d::FREE_SPACE){
00304         update_count++;
00305       }
00306       printf("%c", printableCost( map.getCost( i, j )));
00307     }
00308     printf("\n");
00309   }
00310 
00311   ASSERT_EQ(update_count, 79);
00312 }
00313 
00315 TEST(costmap, testWindowCopy){
00316   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
00317       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00318 
00319   /*
00320   for(unsigned int i = 0; i < 10; ++i){
00321     for(unsigned int j = 0; j < 10; ++j){
00322       printf("%3d ", map.getCost(i, j));
00323     }
00324     printf("\n");
00325   }
00326   printf("\n");
00327   */
00328 
00329   Costmap2D windowCopy;
00330 
00331   //first test that if we try to make a window that is too big, things fail
00332   windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 12.0);
00333   ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)0);
00334   ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)0);
00335 
00336   //Next, test that trying to make a map window itself fails
00337   map.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
00338   ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10);
00339   ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
00340 
00341   //Next, test that legal input generates the result that we expect
00342   windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
00343   ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6);
00344   ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6);
00345 
00346   //check that we actually get the windo that we expect
00347   for(unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i){
00348     for(unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j){
00349       ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2));
00350       //printf("%3d ", windowCopy.getCost(i, j));
00351     }
00352     //printf("\n");
00353   }
00354 
00355 }
00356 
00357 //test for updating costmaps with static data
00358 TEST(costmap, testFullyContainedStaticMapUpdate){
00359   Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
00360       10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD);
00361 
00362   Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
00363       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00364 
00365   map.updateStaticMapWindow(0, 0, 10, 10, MAP_10_BY_10);
00366 
00367   for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){
00368     for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){
00369       ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j));
00370     }
00371   }
00372 }
00373 
00374 TEST(costmap, testOverlapStaticMapUpdate){
00375   Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
00376       10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD);
00377 
00378   Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
00379       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00380 
00381   map.updateStaticMapWindow(-10, -10, 10, 10, MAP_10_BY_10);
00382 
00383   ASSERT_FLOAT_EQ(map.getOriginX(), -10);
00384   ASSERT_FLOAT_EQ(map.getOriginX(), -10);
00385   ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15);
00386   ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15);
00387   for(unsigned int i = 0; i < 10; ++i){
00388     for(unsigned int j = 0; j < 10; ++j){
00389       ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j));
00390     }
00391   }
00392 
00393   std::vector<unsigned char> blank(100);
00394 
00395   //check to make sure that inflation on updates are being done correctly
00396   map.updateStaticMapWindow(-10, -10, 10, 10, blank);
00397 
00398   for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){
00399     for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){
00400       ASSERT_EQ(map.getCost(i, j), 0);
00401     }
00402   }
00403 
00404   std::vector<unsigned char> fully_contained(25);
00405   fully_contained[0] = 254;
00406   fully_contained[4] = 254;
00407   fully_contained[5] = 254;
00408   fully_contained[9] = 254;
00409 
00410   Costmap2D small_static_map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
00411       10.0, MAX_Z, 10.0, 25, fully_contained, THRESHOLD);
00412 
00413   map.updateStaticMapWindow(0, 0, 5, 5, fully_contained);
00414 
00415   ASSERT_FLOAT_EQ(map.getOriginX(), -10);
00416   ASSERT_FLOAT_EQ(map.getOriginX(), -10);
00417   ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15);
00418   ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15);
00419   for(unsigned int j = 0; j < 5; ++j){
00420     for(unsigned int i = 0; i < 5; ++i){
00421       ASSERT_EQ(map.getCost(i + 10, j + 10), small_static_map.getCost(i, j));
00422     }
00423   }
00424 
00425 }
00426 
00430 TEST(costmap, testRaytracing){
00431   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
00432       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00433 
00434   // Add a point cloud, should not affect the map
00435   pcl::PointCloud<pcl::PointXYZ> cloud;
00436   cloud.points.resize(1);
00437   cloud.points[0].x = 0;
00438   cloud.points[0].y = 0;
00439   cloud.points[0].z = MAX_Z;
00440 
00441   geometry_msgs::Point p;
00442   p.x = 0.0;
00443   p.y = 0.0;
00444   p.z = MAX_Z;
00445 
00446   Observation obs(p, cloud, 100.0, 100.0);
00447   std::vector<Observation> obsBuf;
00448   obsBuf.push_back(obs);
00449 
00450   map.updateWorld(0, 0, obsBuf, obsBuf);
00451 
00452   int lethal_count = 0;
00453 
00454   for(unsigned int i = 0; i < 10; ++i){
00455     for(unsigned int j = 0; j < 10; ++j){
00456       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
00457         lethal_count++;
00458       }
00459     }
00460   }
00461 
00462   //we expect just one obstacle to be added
00463   ASSERT_EQ(lethal_count, 21);
00464 }
00465 
00466 TEST(costmap, testAdjacentToObstacleCanStillMove){
00467   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, 2.1, 3.1, 4.1, 
00468                 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00469   pcl::PointCloud<pcl::PointXYZ> cloud;
00470   cloud.points.resize(1);
00471   cloud.points[0].x = 0;
00472   cloud.points[0].y = 0;
00473   cloud.points[0].z = MAX_Z;
00474 
00475   geometry_msgs::Point p;
00476   p.x = 0.0;
00477   p.y = 0.0;
00478   p.z = MAX_Z;
00479 
00480   Observation obs(p, cloud, 100.0, 100.0);
00481   std::vector<Observation> obsBuf;
00482   obsBuf.push_back(obs);
00483 
00484   map.updateWorld(9, 9, obsBuf, obsBuf);
00485 
00486   EXPECT_EQ( LETHAL_OBSTACLE, map.getCost( 0, 0 ));
00487   EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 1, 0 ));
00488   EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 2, 0 ));
00489   EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > map.getCost( 3, 0 ));
00490   EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > map.getCost( 2, 1 ));
00491   EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 1, 1 ));
00492 }
00493 
00494 TEST(costmap, testInflationShouldNotCreateUnknowns){
00495   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, 2.1, 3.1, 4.1, 
00496                 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00497   pcl::PointCloud<pcl::PointXYZ> cloud;
00498   cloud.points.resize(1);
00499   cloud.points[0].x = 0;
00500   cloud.points[0].y = 0;
00501   cloud.points[0].z = MAX_Z;
00502 
00503   geometry_msgs::Point p;
00504   p.x = 0.0;
00505   p.y = 0.0;
00506   p.z = MAX_Z;
00507 
00508   Observation obs(p, cloud, 100.0, 100.0);
00509   std::vector<Observation> obsBuf;
00510   obsBuf.push_back(obs);
00511 
00512   map.updateWorld(9, 9, obsBuf, obsBuf);
00513 
00514   int unknown_count = 0;
00515 
00516   for(unsigned int i = 0; i < 10; ++i){
00517     for(unsigned int j = 0; j < 10; ++j){
00518       if(map.getCost(i, j) == costmap_2d::NO_INFORMATION){
00519         unknown_count++;
00520       }
00521     }
00522   }
00523   EXPECT_EQ( 0, unknown_count );
00524 }
00525 
00526 unsigned int worldToIndex(Costmap2D& map, double wx, double wy){
00527   unsigned int mx, my;
00528   map.worldToMap(wx, wy, mx, my);
00529   return map.getIndex(mx, my);
00530 }
00531 
00532 void indexToWorld(Costmap2D& map, unsigned int index, double& wx, double& wy){
00533   unsigned int mx, my;
00534   map.indexToCells(index, mx, my);
00535   map.mapToWorld(mx, my, wx, wy);
00536 }
00537 
00538 TEST(costmap, testStaticMap){
00539   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
00540       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00541 
00542   ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10);
00543   ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
00544 
00545   // Verify that obstacles correctly identified from the static map.
00546   std::vector<unsigned int> occupiedCells;
00547 
00548   for(unsigned int i = 0; i < 10; ++i){
00549     for(unsigned int j = 0; j < 10; ++j){
00550       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
00551         occupiedCells.push_back(map.getIndex(i, j));
00552       }
00553     }
00554   }
00555 
00556   ASSERT_EQ(occupiedCells.size(), (unsigned int)20);
00557 
00558   // Iterate over all id's and verify that they are present according to their
00559   for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
00560     unsigned int ind = *it;
00561     unsigned int x, y;
00562     map.indexToCells(ind, x, y);
00563     ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
00564     ASSERT_EQ(MAP_10_BY_10[ind] >= 100, true);
00565     ASSERT_EQ(map.getCost(x, y) >= 100, true);
00566   }
00567 
00568   // Block of 200
00569   ASSERT_EQ(find(occupiedCells, map.getIndex(7, 2)), true);
00570   ASSERT_EQ(find(occupiedCells, map.getIndex(8, 2)), true);
00571   ASSERT_EQ(find(occupiedCells, map.getIndex(9, 2)), true);
00572   ASSERT_EQ(find(occupiedCells, map.getIndex(7, 3)), true);
00573   ASSERT_EQ(find(occupiedCells, map.getIndex(8, 3)), true);
00574   ASSERT_EQ(find(occupiedCells, map.getIndex(9, 3)), true);
00575   ASSERT_EQ(find(occupiedCells, map.getIndex(7, 4)), true);
00576   ASSERT_EQ(find(occupiedCells, map.getIndex(8, 4)), true);
00577   ASSERT_EQ(find(occupiedCells, map.getIndex(9, 4)), true);
00578 
00579   // Block of 100
00580   ASSERT_EQ(find(occupiedCells, map.getIndex(4, 3)), true);
00581   ASSERT_EQ(find(occupiedCells, map.getIndex(4, 4)), true);
00582 
00583   // Block of 200
00584   ASSERT_EQ(find(occupiedCells, map.getIndex(3, 7)), true);
00585   ASSERT_EQ(find(occupiedCells, map.getIndex(4, 7)), true);
00586   ASSERT_EQ(find(occupiedCells, map.getIndex(5, 7)), true);
00587 
00588 
00589   // Verify Coordinate Transformations, ROW MAJOR ORDER
00590   ASSERT_EQ(worldToIndex(map, 0.0, 0.0), (unsigned int)0);
00591   ASSERT_EQ(worldToIndex(map, 0.0, 0.99), (unsigned int)0);
00592   ASSERT_EQ(worldToIndex(map, 0.0, 1.0), (unsigned int)10);
00593   ASSERT_EQ(worldToIndex(map, 1.0, 0.99), (unsigned int)1);
00594   ASSERT_EQ(worldToIndex(map, 9.99, 9.99), (unsigned int)99);
00595   ASSERT_EQ(worldToIndex(map, 8.2, 3.4), (unsigned int)38);
00596 
00597   // Ensure we hit the middle of the cell for world co-ordinates
00598   double wx, wy;
00599   indexToWorld(map, 99, wx, wy);
00600   ASSERT_EQ(wx, 9.5);
00601   ASSERT_EQ(wy, 9.5);
00602 }
00603 
00604 
00609 TEST(costmap, testDynamicObstacles){
00610   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
00611       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00612 
00613   // Add a point cloud and verify its insertion. There should be only one new one
00614   pcl::PointCloud<pcl::PointXYZ> cloud;
00615   cloud.points.resize(3);
00616   cloud.points[0].x = 0;
00617   cloud.points[0].y = 0;
00618   cloud.points[1].x = 0;
00619   cloud.points[1].y = 0;
00620   cloud.points[2].x = 0;
00621   cloud.points[2].y = 0;
00622 
00623   geometry_msgs::Point p;
00624   p.x = 0.0;
00625   p.y = 0.0;
00626   p.z = MAX_Z;
00627 
00628   Observation obs(p, cloud, 100.0, 100.0);
00629   std::vector<Observation> obsBuf;
00630   obsBuf.push_back(obs);
00631 
00632   map.updateWorld(0, 0, obsBuf, obsBuf);
00633 
00634   std::vector<unsigned int> ids;
00635 
00636   for(unsigned int i = 0; i < 10; ++i){
00637     for(unsigned int j = 0; j < 10; ++j){
00638       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
00639         ids.push_back(map.getIndex(i, j));
00640       }
00641     }
00642   }
00643 
00644   // Should now have 1 insertion and no deletions
00645   ASSERT_EQ(ids.size(), (unsigned int)21);
00646 
00647   // Repeating the call - we should see no insertions or deletions
00648   map.updateWorld(0, 0, obsBuf, obsBuf);
00649   ASSERT_EQ(ids.size(), (unsigned int)21);
00650 }
00651 
00655 TEST(costmap, testMultipleAdditions){
00656   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
00657       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00658 
00659   // A point cloud with one point that falls within an existing obstacle
00660   pcl::PointCloud<pcl::PointXYZ> cloud;
00661   cloud.points.resize(1);
00662   cloud.points[0].x = 7;
00663   cloud.points[0].y = 2;
00664 
00665   geometry_msgs::Point p;
00666   p.x = 0.0;
00667   p.y = 0.0;
00668   p.z = MAX_Z;
00669 
00670   Observation obs(p, cloud, 100.0, 100.0);
00671   std::vector<Observation> obsBuf;
00672   obsBuf.push_back(obs);
00673 
00674   map.updateWorld(0, 0, obsBuf, obsBuf);
00675 
00676   std::vector<unsigned int> ids;
00677 
00678   for(unsigned int i = 0; i < 10; ++i){
00679     for(unsigned int j = 0; j < 10; ++j){
00680       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
00681         ids.push_back(map.getIndex(i, j));
00682       }
00683     }
00684   }
00685 
00686   ASSERT_EQ(ids.size(), (unsigned int)20);
00687 }
00688 
00692 TEST(costmap, testZThreshold){
00693   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
00694       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00695 
00696   // A point cloud with 2 points falling in a cell with a non-lethal cost
00697   pcl::PointCloud<pcl::PointXYZ> c0;
00698   c0.points.resize(2);
00699   c0.points[0].x = 0;
00700   c0.points[0].y = 5;
00701   c0.points[0].z = 0.4;
00702   c0.points[1].x = 1;
00703   c0.points[1].y = 5;
00704   c0.points[1].z = 1.2;
00705 
00706   geometry_msgs::Point p;
00707   p.x = 0.0;
00708   p.y = 0.0;
00709   p.z = MAX_Z;
00710 
00711   Observation obs(p, c0, 100.0, 100.0);
00712   std::vector<Observation> obsBuf;
00713   obsBuf.push_back(obs);
00714 
00715   map.updateWorld(0, 0, obsBuf, obsBuf);
00716 
00717   std::vector<unsigned int> ids;
00718 
00719   for(unsigned int i = 0; i < 10; ++i){
00720     for(unsigned int j = 0; j < 10; ++j){
00721       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
00722         ids.push_back(map.getIndex(i, j));
00723       }
00724     }
00725   }
00726 
00727   ASSERT_EQ(ids.size(), (unsigned int)21);
00728 }
00729 
00734 TEST(costmap, testInflation){
00735   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
00736       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00737 
00738   // Verify that obstacles correctly identified
00739   std::vector<unsigned int> occupiedCells;
00740 
00741   for(unsigned int i = 0; i < 10; ++i){
00742     for(unsigned int j = 0; j < 10; ++j){
00743       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00744         occupiedCells.push_back(map.getIndex(i, j));
00745       }
00746     }
00747   }
00748 
00749   // There should be no duplicates
00750   std::set<unsigned int> setOfCells;
00751   for(unsigned int i=0;i<occupiedCells.size(); i++)
00752     setOfCells.insert(i);
00753 
00754   ASSERT_EQ(setOfCells.size(), occupiedCells.size());
00755   ASSERT_EQ(setOfCells.size(), (unsigned int)48);
00756 
00757   // Iterate over all id's and verify they are obstacles
00758   for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
00759     unsigned int ind = *it;
00760     unsigned int x, y;
00761     map.indexToCells(ind, x, y);
00762     ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
00763     ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
00764   }
00765 
00766   // Set an obstacle at the origin and observe insertions for it and its neighbors
00767   pcl::PointCloud<pcl::PointXYZ> c0;
00768   c0.points.resize(1);
00769   c0.points[0].x = 0;
00770   c0.points[0].y = 0;
00771   c0.points[0].z = 0.4;
00772 
00773   geometry_msgs::Point p;
00774   p.x = 0.0;
00775   p.y = 0.0;
00776   p.z = MAX_Z;
00777 
00778   Observation obs(p, c0, 100.0, 100.0);
00779   std::vector<Observation> obsBuf, empty;
00780   obsBuf.push_back(obs);
00781 
00782   map.updateWorld(0, 0, obsBuf, empty);
00783 
00784   occupiedCells.clear();
00785   for(unsigned int i = 0; i < 10; ++i){
00786     for(unsigned int j = 0; j < 10; ++j){
00787       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00788         occupiedCells.push_back(map.getIndex(i, j));
00789       }
00790     }
00791   }
00792 
00793   // It and its 2 neighbors makes 3 obstacles
00794   ASSERT_EQ(occupiedCells.size(), (unsigned int)51);
00795 
00796   // @todo Rewrite 
00797   // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
00798   pcl::PointCloud<pcl::PointXYZ> c1;
00799   c1.points.resize(1);
00800   c1.points[0].x = 2;
00801   c1.points[0].y = 0;
00802   c1.points[0].z = 0.0;
00803 
00804   geometry_msgs::Point p1;
00805   p1.x = 0.0;
00806   p1.y = 0.0;
00807   p1.z = MAX_Z;
00808 
00809   Observation obs1(p1, c1, 100.0, 100.0);
00810   std::vector<Observation> obsBuf1;
00811   obsBuf1.push_back(obs1);
00812 
00813   map.updateWorld(0, 0, obsBuf1, empty);
00814 
00815   occupiedCells.clear();
00816   for(unsigned int i = 0; i < 10; ++i){
00817     for(unsigned int j = 0; j < 10; ++j){
00818       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00819         occupiedCells.push_back(map.getIndex(i, j));
00820       }
00821     }
00822   }
00823 
00824   // Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from
00825   // the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle
00826   // at <0, 1>
00827   ASSERT_EQ(occupiedCells.size(), (unsigned int)54);
00828 
00829 
00830   // Add an obstacle at <1, 9>. This will inflate obstacles around it
00831   pcl::PointCloud<pcl::PointXYZ> c2;
00832   c2.points.resize(1);
00833   c2.points[0].x = 1;
00834   c2.points[0].y = 9;
00835   c2.points[0].z = 0.0;
00836 
00837   geometry_msgs::Point p2;
00838   p2.x = 0.0;
00839   p2.y = 0.0;
00840   p2.z = MAX_Z;
00841 
00842   Observation obs2(p2, c2, 100.0, 100.0);
00843   std::vector<Observation> obsBuf2;
00844   obsBuf2.push_back(obs2);
00845 
00846   map.updateWorld(0, 0, obsBuf2, empty);
00847 
00848   ASSERT_EQ(map.getCost(1, 9), costmap_2d::LETHAL_OBSTACLE);
00849   ASSERT_EQ(map.getCost(0, 9), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
00850   ASSERT_EQ(map.getCost(2, 9), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
00851 
00852   // Add an obstacle and verify that it over-writes its inflated status
00853   pcl::PointCloud<pcl::PointXYZ> c3;
00854   c3.points.resize(1);
00855   c3.points[0].x = 0;
00856   c3.points[0].y = 9;
00857   c3.points[0].z = 0.0;
00858 
00859   geometry_msgs::Point p3;
00860   p3.x = 0.0;
00861   p3.y = 0.0;
00862   p3.z = MAX_Z;
00863 
00864   Observation obs3(p3, c3, 100.0, 100.0);
00865   std::vector<Observation> obsBuf3;
00866   obsBuf3.push_back(obs3);
00867 
00868   map.updateWorld(0, 0, obsBuf3, empty);
00869 
00870   ASSERT_EQ(map.getCost(0, 9), costmap_2d::LETHAL_OBSTACLE);
00871 }
00872 
00876 TEST(costmap, testInflation2){
00877   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
00878       10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
00879 
00880   // Creat a small L-Shape all at once
00881   pcl::PointCloud<pcl::PointXYZ> c0;
00882   c0.points.resize(3);
00883   c0.points[0].x = 1;
00884   c0.points[0].y = 1;
00885   c0.points[0].z = MAX_Z;
00886   c0.points[1].x = 1;
00887   c0.points[1].y = 2;
00888   c0.points[1].z = MAX_Z;
00889   c0.points[2].x = 2;
00890   c0.points[2].y = 2;
00891   c0.points[2].z = MAX_Z;
00892 
00893   geometry_msgs::Point p;
00894   p.x = 0.0;
00895   p.y = 0.0;
00896   p.z = MAX_Z;
00897 
00898   Observation obs(p, c0, 100.0, 100.0);
00899   std::vector<Observation> obsBuf;
00900   obsBuf.push_back(obs);
00901 
00902   map.updateWorld(0, 0, obsBuf, obsBuf);
00903 
00904   ASSERT_EQ(map.getCost(3, 2), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);  
00905   ASSERT_EQ(map.getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
00906 }
00907 
00911 TEST(costmap, testInflation3){
00912   std::vector<unsigned char> mapData;
00913   for(unsigned int i=0; i< GRID_WIDTH; i++){
00914     for(unsigned int j = 0; j < GRID_HEIGHT; j++){
00915       mapData.push_back(0);
00916     }
00917   }
00918 
00919   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS * 2, ROBOT_RADIUS * 3, 
00920       10.0, MAX_Z, 10.0, 1, mapData, THRESHOLD);
00921 
00922   // There should be no occupied cells
00923   std::vector<unsigned int> ids;
00924 
00925   for(unsigned int i = 0; i < 10; ++i){
00926     for(unsigned int j = 0; j < 10; ++j){
00927       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00928         ids.push_back(map.getIndex(i, j));
00929       }
00930     }
00931   }
00932 
00933   ASSERT_EQ(ids.size(), (unsigned int)0);
00934 
00935   // Add an obstacle at 5,5
00936   pcl::PointCloud<pcl::PointXYZ> c0;
00937   c0.points.resize(1);
00938   c0.points[0].x = 5;
00939   c0.points[0].y = 5;
00940   c0.points[0].z = MAX_Z;
00941 
00942   geometry_msgs::Point p;
00943   p.x = 0.0;
00944   p.y = 0.0;
00945   p.z = MAX_Z;
00946 
00947   Observation obs(p, c0, 100.0, 100.0);
00948   std::vector<Observation> obsBuf;
00949   obsBuf.push_back(obs);
00950 
00951   map.updateWorld(0, 0, obsBuf, obsBuf);
00952 
00953   for(unsigned int i = 0; i < 10; ++i){
00954     for(unsigned int j = 0; j < 10; ++j){
00955       if(map.getCost(i, j) != costmap_2d::FREE_SPACE){
00956         ids.push_back(map.getIndex(i, j));
00957       }
00958     }
00959   }
00960 
00961   ASSERT_EQ(ids.size(), (unsigned int)29);
00962 
00963   ids.clear();
00964   for(unsigned int i = 0; i < 10; ++i){
00965     for(unsigned int j = 0; j < 10; ++j){
00966       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00967         ids.push_back(map.getIndex(i, j));
00968       }
00969     }
00970   }
00971 
00972   ASSERT_EQ(ids.size(), (unsigned int)5);
00973 
00974   // Update again - should see no change
00975   map.updateWorld(0, 0, obsBuf, obsBuf);
00976 
00977   ids.clear();
00978   for(unsigned int i = 0; i < 10; ++i){
00979     for(unsigned int j = 0; j < 10; ++j){
00980       if(map.getCost(i, j) != costmap_2d::FREE_SPACE){
00981         ids.push_back(map.getIndex(i, j));
00982       }
00983     }
00984   }
00985   
00986   ASSERT_EQ(ids.size(), (unsigned int)29);
00987 }
00988 
00993 TEST(costmap, testRaytracing2){
00994   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
00995       100.0, MAX_Z, 100.0, 1, MAP_10_BY_10, THRESHOLD);
00996 
00997   // The sensor origin will be <0,0>. So if we add an obstacle at 9,9, we would expect cells
00998   // <0, 0> thru <8, 8> to be traced through
00999   pcl::PointCloud<pcl::PointXYZ> c0;
01000   c0.points.resize(1);
01001   c0.points[0].x = 9.5;
01002   c0.points[0].y = 9.5;
01003   c0.points[0].z = MAX_Z;
01004 
01005   geometry_msgs::Point p;
01006   p.x = 0.5;
01007   p.y = 0.5;
01008   p.z = MAX_Z;
01009 
01010   Observation obs(p, c0, 100.0, 100.0);
01011   std::vector<Observation> obsBuf;
01012   obsBuf.push_back(obs);
01013 
01014   std::vector<unsigned int> obstacles;
01015 
01016   for(unsigned int i = 0; i < 10; ++i){
01017     for(unsigned int j = 0; j < 10; ++j){
01018       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
01019         obstacles.push_back(map.getIndex(i, j));
01020       }
01021     }
01022   }
01023 
01024   unsigned int obs_before = obstacles.size();
01025 
01026   map.updateWorld(0, 0, obsBuf, obsBuf);
01027 
01028   obstacles.clear();
01029   for(unsigned int i = 0; i < 10; ++i){
01030     for(unsigned int j = 0; j < 10; ++j){
01031       if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
01032         obstacles.push_back(map.getIndex(i, j));
01033       }
01034     }
01035   }
01036 
01037   //Two obstacles shoulb be removed from the map by raytracing
01038   ASSERT_EQ(obstacles.size(), obs_before - 2);
01039 
01040 
01041   // many cells will have been switched to free space along the diagonal except
01042   // for those inflated in the update.. tests that inflation happens properly
01043   // after raytracing
01044   unsigned char test[10]= {0, 0, 0, 253, 253, 0, 0, 253, 253, 254};
01045   for(unsigned int i=0; i < 10; i++)
01046     ASSERT_EQ(map.getCost(i, i), test[i]);
01047 }
01048 
01055 TEST(costmap, testTrickyPropagation){
01056   const unsigned char MAP_HALL_CHAR[10 * 10] = {
01057     0,   0, 0,   0,   0, 0,   0, 0, 0, 0,
01058     254, 0, 0,   0,   0, 0,   0, 0, 0, 0,
01059     0,   0, 0,   0,   0, 0,   0, 0, 0, 0,
01060     0,   0, 0,   254, 0, 0,   0, 0, 0, 0,
01061     0,   0, 0,   0,   0, 0,   0, 0, 0, 0,
01062     0,   0, 0,   0,   0, 0,   0, 0, 0, 0,
01063     0,   0, 0,   0,   0, 254, 0, 0, 0, 0,
01064     0,   0, 0,   0,   0, 254, 0, 0, 0, 0,
01065     0,   0, 0,   0,   0, 0,   0, 0, 0, 0,
01066     0,   0, 0,   0,   0, 0,   0, 0, 0, 0,
01067   };
01068   std::vector<unsigned char> MAP_HALL;
01069   for (int i = 0; i < 10 * 10; i++) {
01070     MAP_HALL.push_back(MAP_HALL_CHAR[i]);
01071   }
01072 
01073   Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 
01074       100.0, MAX_Z, 100.0, 1, MAP_HALL, THRESHOLD);
01075 
01076 
01077   //Add a dynamic obstacle
01078   pcl::PointCloud<pcl::PointXYZ> c2;
01079   c2.points.resize(3);
01080   //Dynamic obstacle that raytaces.
01081   c2.points[0].x = 7.0;
01082   c2.points[0].y = 8.0;
01083   c2.points[0].z = 1.0;
01084   //Dynamic obstacle that should not be raytraced the
01085   //first update, but should on the second.
01086   c2.points[1].x = 3.0;
01087   c2.points[1].y = 4.0;
01088   c2.points[1].z = 1.0;
01089   //Dynamic obstacle that should not be erased.
01090   c2.points[2].x = 6.0;
01091   c2.points[2].y = 3.0;
01092   c2.points[2].z = 1.0;
01093 
01094   geometry_msgs::Point p2;
01095   p2.x = 0.5;
01096   p2.y = 0.5;
01097   p2.z = MAX_Z;
01098 
01099   Observation obs2(p2, c2, 100.0, 100.0);
01100   std::vector<Observation> obsBuf2;
01101   obsBuf2.push_back(obs2);
01102 
01103   map.updateWorld(0, 0, obsBuf2, obsBuf2);
01104 
01105   const unsigned char MAP_HALL_CHAR_TEST[10 * 10] = { 
01106     253, 254, 253,   0,   0,   0,   0,   0,   0,   0,
01107       0, 253,   0,   0,   0,   0,   0,   0,   0,   0,
01108       0,   0,   0,   0, 253,   0,   0,   0,   0,   0,
01109       0,   0,   0, 253, 254, 253,   0,   0,   0,   0,
01110       0,   0,   0,   0, 253,   0,   0, 253,   0,   0,
01111       0,   0,   0, 253,   0,   0, 253, 254, 253,   0,
01112       0,   0, 253, 254, 253,   0,   0, 253, 253,   0,
01113       0,   0,   0, 253,   0,   0,   0, 253, 254, 253,
01114       0,   0,   0,   0,   0,   0,   0,   0, 253,   0,
01115       0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
01116   };
01117 
01118   
01119   for (int i = 0; i < 10 * 10; i++) {
01120     ASSERT_EQ(map.getCost(i / 10, i % 10), MAP_HALL_CHAR_TEST[i]);
01121   }
01122 
01123   pcl::PointCloud<pcl::PointXYZ> c;
01124   c.points.resize(1);
01125   //Dynamic obstacle that raytaces the one at (3.0, 4.0).
01126   c.points[0].x = 4.0;
01127   c.points[0].y = 5.0;
01128   c.points[0].z = 1.0;
01129 
01130   geometry_msgs::Point p3;
01131   p3.x = 0.5;
01132   p3.y = 0.5;
01133   p3.z = MAX_Z;
01134 
01135   Observation obs3(p3, c, 100.0, 100.0);
01136   std::vector<Observation> obsBuf3;
01137   obsBuf3.push_back(obs3);
01138 
01139   map.updateWorld(0, 0, obsBuf3, obsBuf3);
01140 
01141   const unsigned char MAP_HALL_CHAR_TEST2[10 * 10] = { 
01142     253, 254, 253,   0,   0,   0,   0,   0,   0,   0,
01143       0, 253,   0,   0,   0,   0,   0,   0,   0,   0,
01144       0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
01145       0,   0,   0,   0,   0, 253,   0,   0,   0,   0,
01146       0,   0,   0,   0, 253, 254, 253, 253,   0,   0,
01147       0,   0,   0, 253,   0, 253, 253, 254, 253,   0,
01148       0,   0, 253, 254, 253,   0,   0, 253, 253,   0,
01149       0,   0,   0, 253,   0,   0,   0, 253, 254, 253,
01150       0,   0,   0,   0,   0,   0,   0,   0, 253,   0,
01151       0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
01152   };
01153 
01154   
01155   for (int i = 0; i < 10 * 10; i++) {
01156     ASSERT_EQ(map.getCost(i / 10, i % 10), MAP_HALL_CHAR_TEST2[i]);
01157   }
01158 }
01159 
01160 
01161 
01162 int main(int argc, char** argv){
01163   for(unsigned int i = 0; i< GRID_WIDTH * GRID_HEIGHT; i++){
01164     EMPTY_10_BY_10.push_back(0);
01165     MAP_10_BY_10.push_back(MAP_10_BY_10_CHAR[i]);
01166   }
01167 
01168   for(unsigned int i = 0; i< 5 * 5; i++){
01169     MAP_5_BY_5.push_back(MAP_10_BY_10_CHAR[i]);
01170   }
01171 
01172   for(unsigned int i = 0; i< 100 * 100; i++)
01173     EMPTY_100_BY_100.push_back(0);
01174 
01175   testing::InitGoogleTest(&argc, argv);
01176   return RUN_ALL_TESTS();
01177 }


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