utest.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 #include <gtest/gtest.h>
00035 #include <iostream>
00036 #include <vector>
00037 #include <utility>
00038 #include <base_local_planner/map_cell.h>
00039 #include <base_local_planner/map_grid.h>
00040 #include <base_local_planner/trajectory.h>
00041 #include <base_local_planner/trajectory_planner.h>
00042 #include <base_local_planner/costmap_model.h>
00043 #include <costmap_2d/costmap_2d.h>
00044 #include <math.h>
00045 
00046 #include <geometry_msgs/Point.h>
00047 #include <base_local_planner/Position2DInt.h>
00048 
00049 
00050 using namespace std;
00051 using namespace base_local_planner;
00052 
00053 namespace base_local_planner {
00054   class WavefrontMapAccessor : public costmap_2d::Costmap2D {
00055     public:
00056       WavefrontMapAccessor(MapGrid &map, double outer_radius) 
00057         : costmap_2d::Costmap2D(map.size_x_, map.size_y_, map.scale, map.origin_x, map.origin_y, 5, 10, 15),
00058         map_(map), outer_radius_(outer_radius) {
00059           synchronize();
00060         }
00061 
00062       virtual ~WavefrontMapAccessor(){};
00063 
00064       void synchronize(){
00065         // Write Cost Data from the map
00066         for(unsigned int x = 0; x < size_x_; x++){
00067           for (unsigned int y = 0; y < size_y_; y++){
00068             unsigned int ind = x + (y * size_x_);
00069             if(map_(x, y).occ_state == 1)
00070               costmap_[ind] = costmap_2d::LETHAL_OBSTACLE;
00071             else if(map_(x, y).occ_dist < outer_radius_)
00072               costmap_[ind] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE/2;
00073             else 
00074               costmap_[ind] = 0;
00075           }
00076         }
00077       }
00078 
00079     private:
00080       MapGrid& map_;
00081       double outer_radius_;
00082   };
00083 
00084   class TrajectoryPlannerTest : public testing::Test {
00085     public:
00086     TrajectoryPlannerTest(MapGrid& g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec);
00087     void correctFootprint();
00088     void footprintObstacles();
00089     void checkGoalDistance();
00090     void checkPathDistance();
00091     virtual void TestBody(){}
00092 
00093     MapGrid& map_;
00094     WavefrontMapAccessor* wa;
00095     CostmapModel cm;
00096     TrajectoryPlanner tc;
00097   };
00098 
00099   TrajectoryPlannerTest::TrajectoryPlannerTest(MapGrid& g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec) 
00100     : map_(g), wa(wave), cm(map), tc(cm, map, footprint_spec, 0.0, 1.0, 1.0, 1.0, 1.0, 2.0) 
00101   {}
00102 
00103   void TrajectoryPlannerTest::correctFootprint(){
00104     //just create a basic footprint
00105     vector<base_local_planner::Position2DInt> footprint = tc.getFootprintCells(4.5, 4.5, 0, false);
00106 
00107     //we expect the front line to be first
00108     EXPECT_EQ(footprint[0].x, 6); EXPECT_EQ(footprint[0].y, 6);
00109     EXPECT_EQ(footprint[1].x, 6); EXPECT_EQ(footprint[1].y, 5);
00110     EXPECT_EQ(footprint[2].x, 6); EXPECT_EQ(footprint[2].y, 4);
00111     EXPECT_EQ(footprint[3].x, 6); EXPECT_EQ(footprint[3].y, 3);
00112     EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 2);
00113 
00114     //next the right line
00115     EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 2);
00116     EXPECT_EQ(footprint[6].x, 5); EXPECT_EQ(footprint[6].y, 2);
00117     EXPECT_EQ(footprint[7].x, 4); EXPECT_EQ(footprint[7].y, 2);
00118     EXPECT_EQ(footprint[8].x, 3); EXPECT_EQ(footprint[8].y, 2);
00119     EXPECT_EQ(footprint[9].x, 2); EXPECT_EQ(footprint[9].y, 2);
00120 
00121     //next the back line
00122     EXPECT_EQ(footprint[10].x, 2); EXPECT_EQ(footprint[10].y, 2);
00123     EXPECT_EQ(footprint[11].x, 2); EXPECT_EQ(footprint[11].y, 3);
00124     EXPECT_EQ(footprint[12].x, 2); EXPECT_EQ(footprint[12].y, 4);
00125     EXPECT_EQ(footprint[13].x, 2); EXPECT_EQ(footprint[13].y, 5);
00126     EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 6);
00127 
00128     //finally the left line
00129     EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 6);
00130     EXPECT_EQ(footprint[16].x, 3); EXPECT_EQ(footprint[16].y, 6);
00131     EXPECT_EQ(footprint[17].x, 4); EXPECT_EQ(footprint[17].y, 6);
00132     EXPECT_EQ(footprint[18].x, 5); EXPECT_EQ(footprint[18].y, 6);
00133     EXPECT_EQ(footprint[19].x, 6); EXPECT_EQ(footprint[19].y, 6);
00134 
00135     //check that rotation of the footprint works
00136     footprint = tc.getFootprintCells(4.5, 4.5, M_PI_2, false);
00137 
00138     //first the left line
00139     EXPECT_EQ(footprint[0].x, 2); EXPECT_EQ(footprint[0].y, 6);
00140     EXPECT_EQ(footprint[1].x, 3); EXPECT_EQ(footprint[1].y, 6);
00141     EXPECT_EQ(footprint[2].x, 4); EXPECT_EQ(footprint[2].y, 6);
00142     EXPECT_EQ(footprint[3].x, 5); EXPECT_EQ(footprint[3].y, 6);
00143     EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 6);
00144 
00145     //next the front line
00146     EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 6);
00147     EXPECT_EQ(footprint[6].x, 6); EXPECT_EQ(footprint[6].y, 5);
00148     EXPECT_EQ(footprint[7].x, 6); EXPECT_EQ(footprint[7].y, 4);
00149     EXPECT_EQ(footprint[8].x, 6); EXPECT_EQ(footprint[8].y, 3);
00150     EXPECT_EQ(footprint[9].x, 6); EXPECT_EQ(footprint[9].y, 2);
00151 
00152     //next the right line
00153     EXPECT_EQ(footprint[10].x, 6); EXPECT_EQ(footprint[10].y, 2);
00154     EXPECT_EQ(footprint[11].x, 5); EXPECT_EQ(footprint[11].y, 2);
00155     EXPECT_EQ(footprint[12].x, 4); EXPECT_EQ(footprint[12].y, 2);
00156     EXPECT_EQ(footprint[13].x, 3); EXPECT_EQ(footprint[13].y, 2);
00157     EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 2);
00158 
00159     //next the back line
00160     EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 2);
00161     EXPECT_EQ(footprint[16].x, 2); EXPECT_EQ(footprint[16].y, 3);
00162     EXPECT_EQ(footprint[17].x, 2); EXPECT_EQ(footprint[17].y, 4);
00163     EXPECT_EQ(footprint[18].x, 2); EXPECT_EQ(footprint[18].y, 5);
00164     EXPECT_EQ(footprint[19].x, 2); EXPECT_EQ(footprint[19].y, 6);
00165   }
00166 
00167   void TrajectoryPlannerTest::footprintObstacles(){
00168     //place an obstacle
00169     map_(4, 6).occ_state = 1;
00170     wa->synchronize();
00171     EXPECT_EQ(wa->getCost(4,6), costmap_2d::LETHAL_OBSTACLE);
00172     Trajectory traj(0, 0, 0, 30);
00173     //tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj, 2, 30);
00174     tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
00175     //we expect this path to hit the obstacle
00176     EXPECT_FLOAT_EQ(traj.cost_, -1.0);
00177 
00178     //place a wall next to the footprint of the robot
00179     map_(7, 1).occ_state = 1;
00180     map_(7, 3).occ_state = 1;
00181     map_(7, 4).occ_state = 1;
00182     map_(7, 5).occ_state = 1;
00183     map_(7, 6).occ_state = 1;
00184     map_(7, 7).occ_state = 1;
00185     wa->synchronize();
00186 
00187     //try to rotate into it
00188     //tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj, 2, 30);
00189     tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj);
00190     //we expect this path to hit the obstacle
00191     EXPECT_FLOAT_EQ(traj.cost_, -1.0);
00192   }
00193 
00194   void TrajectoryPlannerTest::checkGoalDistance(){
00195     //let's box a cell in and make sure that its distance gets set to max
00196     map_(1, 2).occ_state = 1;
00197     map_(1, 1).occ_state = 1;
00198     map_(1, 0).occ_state = 1;
00199     map_(2, 0).occ_state = 1;
00200     map_(3, 0).occ_state = 1;
00201     map_(3, 1).occ_state = 1;
00202     map_(3, 2).occ_state = 1;
00203     map_(2, 2).occ_state = 1;
00204     wa->synchronize();
00205 
00206     //set a goal
00207     tc.map_.resetPathDist();
00208     queue<MapCell*> goal_dist_queue;
00209     MapCell& current = tc.map_(4, 9);
00210     current.goal_dist = 0.0;
00211     current.goal_mark = true;
00212     goal_dist_queue.push(&current);
00213     tc.map_.computeGoalDistance(goal_dist_queue, tc.costmap_);
00214 
00215     EXPECT_FLOAT_EQ(tc.map_(4, 8).goal_dist, 1.0);
00216     EXPECT_FLOAT_EQ(tc.map_(4, 7).goal_dist, 2.0);
00217     EXPECT_FLOAT_EQ(tc.map_(4, 6).goal_dist, 100.0); //there's an obstacle here placed above
00218     EXPECT_FLOAT_EQ(tc.map_(4, 5).goal_dist, 6.0);
00219     EXPECT_FLOAT_EQ(tc.map_(4, 4).goal_dist, 7.0);
00220     EXPECT_FLOAT_EQ(tc.map_(4, 3).goal_dist, 8.0);
00221     EXPECT_FLOAT_EQ(tc.map_(4, 2).goal_dist, 9.0);
00222     EXPECT_FLOAT_EQ(tc.map_(4, 1).goal_dist, 10.0);
00223     EXPECT_FLOAT_EQ(tc.map_(4, 0).goal_dist, 11.0);
00224     EXPECT_FLOAT_EQ(tc.map_(5, 8).goal_dist, 2.0);
00225     EXPECT_FLOAT_EQ(tc.map_(9, 4).goal_dist, 10.0);
00226 
00227     //check the boxed in cell
00228     EXPECT_FLOAT_EQ(tc.map_(2, 2).goal_dist, 100.0);
00229 
00230   }
00231 
00232   void TrajectoryPlannerTest::checkPathDistance(){
00233     tc.map_.resetPathDist();
00234     queue<MapCell*> path_dist_queue;
00235     MapCell& current = tc.map_(4, 9);
00236     current.path_dist = 0.0;
00237     current.path_mark = true;
00238     path_dist_queue.push(&current);
00239     tc.map_.computePathDistance(path_dist_queue, tc.costmap_);
00240 
00241     EXPECT_FLOAT_EQ(tc.map_(4, 8).path_dist, 1.0);
00242     EXPECT_FLOAT_EQ(tc.map_(4, 7).path_dist, 2.0);
00243     EXPECT_FLOAT_EQ(tc.map_(4, 6).path_dist, 100.0); //there's an obstacle here placed above
00244     EXPECT_FLOAT_EQ(tc.map_(4, 5).path_dist, 6.0);
00245     EXPECT_FLOAT_EQ(tc.map_(4, 4).path_dist, 7.0);
00246     EXPECT_FLOAT_EQ(tc.map_(4, 3).path_dist, 8.0);
00247     EXPECT_FLOAT_EQ(tc.map_(4, 2).path_dist, 9.0);
00248     EXPECT_FLOAT_EQ(tc.map_(4, 1).path_dist, 10.0);
00249     EXPECT_FLOAT_EQ(tc.map_(4, 0).path_dist, 11.0);
00250     EXPECT_FLOAT_EQ(tc.map_(5, 8).path_dist, 2.0);
00251     EXPECT_FLOAT_EQ(tc.map_(9, 4).path_dist, 10.0);
00252 
00253     //check the boxed in cell
00254     EXPECT_FLOAT_EQ(tc.map_(2, 2).path_dist, 100.0);
00255 
00256   }
00257 };
00258 
00259 //sanity check to make sure the grid functions correctly
00260 TEST(MapGrid, properGridConstruction){
00261   MapGrid mg(10, 10);
00262   mg.scale = 1.0;
00263   MapCell mc;
00264 
00265   for(int i = 0; i < 10; ++i){
00266     for(int j = 0; j < 10; ++j){
00267       mc.cx = i;
00268       mc.cy = j;
00269       mg(i, j) = mc;
00270     }
00271   }
00272 
00273   for(int i = 0; i < 10; ++i){
00274     for(int j = 0; j < 10; ++j){
00275       EXPECT_FLOAT_EQ(mg(i, j).cx, i);
00276       EXPECT_FLOAT_EQ(mg(i, j).cy, j);
00277     }
00278   }
00279 }
00280 
00281 TrajectoryPlannerTest* tct = NULL;
00282 
00283 TEST(TrajectoryPlannerTest, correctFootprint){
00284   tct->correctFootprint();
00285 }
00286 
00287 //make sure that trajectories that intersect obstacles are invalidated
00288 TEST(TrajectoryPlannerTest, footprintObstacles){
00289   tct->footprintObstacles();
00290 }
00291 
00292 //make sure that goal distance is being computed as expected
00293 TEST(TrajectoryPlannerTest, checkGoalDistance){
00294   tct->checkGoalDistance();
00295 }
00296 
00297 //make sure that path distance is being computed as expected
00298 TEST(TrajectoryPlannerTest, checkPathDistance){
00299   tct->checkPathDistance();
00300 }
00301 
00302 
00303 
00304 //test some stuff
00305 int main(int argc, char** argv){
00306   MapGrid mg(10, 10, 1, 0, 0);
00307   WavefrontMapAccessor wa(mg, .25);
00308   const costmap_2d::Costmap2D& map = wa;
00309   std::vector<geometry_msgs::Point> footprint_spec;
00310   geometry_msgs::Point pt;
00311   //create a square footprint
00312   pt.x = 2;
00313   pt.y = 2;
00314   footprint_spec.push_back(pt);
00315   pt.x = 2;
00316   pt.y = -2;
00317   footprint_spec.push_back(pt);
00318   pt.x = -2;
00319   pt.y = -2;
00320   footprint_spec.push_back(pt);
00321   pt.x = -2;
00322   pt.y = 2;
00323   footprint_spec.push_back(pt);
00324 
00325   tct = new TrajectoryPlannerTest(mg, &wa, map, footprint_spec);
00326 
00327   testing::InitGoogleTest(&argc, argv);
00328   return RUN_ALL_TESTS();
00329 }


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko
autogenerated on Sat Dec 28 2013 17:13:50