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 
00039 #include <base_local_planner/map_cell.h>
00040 #include <base_local_planner/map_grid.h>
00041 #include <base_local_planner/trajectory.h>
00042 #include <base_local_planner/trajectory_planner.h>
00043 #include <base_local_planner/costmap_model.h>
00044 #include <costmap_2d/costmap_2d.h>
00045 #include <math.h>
00046 
00047 #include <geometry_msgs/Point.h>
00048 #include <base_local_planner/Position2DInt.h>
00049 
00050 #include "wavefront_map_accessor.h"
00051 
00052 using namespace std;
00053 
00054 namespace base_local_planner {
00055 
00056 class TrajectoryPlannerTest : public testing::Test {
00057   public:
00058     TrajectoryPlannerTest(MapGrid* g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec);
00059     void correctFootprint();
00060     void footprintObstacles();
00061     void checkGoalDistance();
00062     void checkPathDistance();
00063     virtual void TestBody(){}
00064 
00065     MapGrid* map_;
00066     WavefrontMapAccessor* wa;
00067     CostmapModel cm;
00068     TrajectoryPlanner tc;
00069 };
00070 
00071 TrajectoryPlannerTest::TrajectoryPlannerTest(MapGrid* g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec)
00072 : map_(g), wa(wave), cm(map), tc(cm, map, footprint_spec, 0.0, 1.0, 1.0, 1.0, 1.0, 2.0)
00073 {}
00074 
00075 
00076 
00077 void TrajectoryPlannerTest::footprintObstacles(){
00078   //place an obstacle
00079   map_->operator ()(4, 6).target_dist = 1;
00080   wa->synchronize();
00081   EXPECT_EQ(wa->getCost(4,6), costmap_2d::LETHAL_OBSTACLE);
00082   Trajectory traj(0, 0, 0, 0.1, 30);
00083   //tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj, 2, 30);
00084   tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
00085   //we expect this path to hit the obstacle
00086   EXPECT_FLOAT_EQ(traj.cost_, -1.0);
00087 
00088   //place a wall next to the footprint of the robot
00089   tc.path_map_(7, 1).target_dist = 1;
00090   tc.path_map_(7, 3).target_dist = 1;
00091   tc.path_map_(7, 4).target_dist = 1;
00092   tc.path_map_(7, 5).target_dist = 1;
00093   tc.path_map_(7, 6).target_dist = 1;
00094   tc.path_map_(7, 7).target_dist = 1;
00095   wa->synchronize();
00096 
00097   //try to rotate into it
00098   //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);
00099   tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj);
00100   //we expect this path to hit the obstacle
00101   EXPECT_FLOAT_EQ(traj.cost_, -1.0);
00102 }
00103 
00104 void TrajectoryPlannerTest::checkGoalDistance(){
00105   //let's box a cell in and make sure that its distance gets set to max
00106   map_->operator ()(1, 2).target_dist = 1;
00107   map_->operator ()(1, 1).target_dist = 1;
00108   map_->operator ()(1, 0).target_dist = 1;
00109   map_->operator ()(2, 0).target_dist = 1;
00110   map_->operator ()(3, 0).target_dist = 1;
00111   map_->operator ()(3, 1).target_dist = 1;
00112   map_->operator ()(3, 2).target_dist = 1;
00113   map_->operator ()(2, 2).target_dist = 1;
00114   wa->synchronize();
00115 
00116   //set a goal
00117   tc.path_map_.resetPathDist();
00118   queue<MapCell*> target_dist_queue;
00119   MapCell& current = tc.path_map_(4, 9);
00120   current.target_dist = 0.0;
00121   current.target_mark = true;
00122   target_dist_queue.push(&current);
00123   tc.path_map_.computeTargetDistance(target_dist_queue, tc.costmap_);
00124 
00125   EXPECT_FLOAT_EQ(tc.path_map_(4, 8).target_dist, 1.0);
00126   EXPECT_FLOAT_EQ(tc.path_map_(4, 7).target_dist, 2.0);
00127   EXPECT_FLOAT_EQ(tc.path_map_(4, 6).target_dist, 100.0); //there's an obstacle here placed above
00128   EXPECT_FLOAT_EQ(tc.path_map_(4, 5).target_dist, 6.0);
00129   EXPECT_FLOAT_EQ(tc.path_map_(4, 4).target_dist, 7.0);
00130   EXPECT_FLOAT_EQ(tc.path_map_(4, 3).target_dist, 8.0);
00131   EXPECT_FLOAT_EQ(tc.path_map_(4, 2).target_dist, 9.0);
00132   EXPECT_FLOAT_EQ(tc.path_map_(4, 1).target_dist, 10.0);
00133   EXPECT_FLOAT_EQ(tc.path_map_(4, 0).target_dist, 11.0);
00134   EXPECT_FLOAT_EQ(tc.path_map_(5, 8).target_dist, 2.0);
00135   EXPECT_FLOAT_EQ(tc.path_map_(9, 4).target_dist, 10.0);
00136 
00137   //check the boxed in cell
00138   EXPECT_FLOAT_EQ(100.0, tc.path_map_(2, 2).target_dist);
00139 
00140 }
00141 
00142 void TrajectoryPlannerTest::checkPathDistance(){
00143   tc.path_map_.resetPathDist();
00144   queue<MapCell*> target_dist_queue;
00145   MapCell& current = tc.path_map_(4, 9);
00146   current.target_dist = 0.0;
00147   current.target_mark = true;
00148   target_dist_queue.push(&current);
00149   tc.path_map_.computeTargetDistance(target_dist_queue, tc.costmap_);
00150 
00151   EXPECT_FLOAT_EQ(tc.path_map_(4, 8).target_dist, 1.0);
00152   EXPECT_FLOAT_EQ(tc.path_map_(4, 7).target_dist, 2.0);
00153   EXPECT_FLOAT_EQ(tc.path_map_(4, 6).target_dist, 100.0); //there's an obstacle here placed above
00154   EXPECT_FLOAT_EQ(tc.path_map_(4, 5).target_dist, 6.0);
00155   EXPECT_FLOAT_EQ(tc.path_map_(4, 4).target_dist, 7.0);
00156   EXPECT_FLOAT_EQ(tc.path_map_(4, 3).target_dist, 8.0);
00157   EXPECT_FLOAT_EQ(tc.path_map_(4, 2).target_dist, 9.0);
00158   EXPECT_FLOAT_EQ(tc.path_map_(4, 1).target_dist, 10.0);
00159   EXPECT_FLOAT_EQ(tc.path_map_(4, 0).target_dist, 11.0);
00160   EXPECT_FLOAT_EQ(tc.path_map_(5, 8).target_dist, 2.0);
00161   EXPECT_FLOAT_EQ(tc.path_map_(9, 4).target_dist, 10.0);
00162 
00163   //check the boxed in cell
00164   EXPECT_FLOAT_EQ(tc.path_map_(2, 2).target_dist, 100.0);
00165 
00166 }
00167 
00168 
00169 TrajectoryPlannerTest* tct = NULL;
00170 
00171 TrajectoryPlannerTest* setup_testclass_singleton() {
00172   if (tct == NULL) {
00173     MapGrid* mg = new MapGrid (10, 10);
00174     WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
00175     const costmap_2d::Costmap2D& map = *wa;
00176     std::vector<geometry_msgs::Point> footprint_spec;
00177     geometry_msgs::Point pt;
00178     //create a square footprint
00179     pt.x = 2;
00180     pt.y = 2;
00181     footprint_spec.push_back(pt);
00182     pt.x = 2;
00183     pt.y = -2;
00184     footprint_spec.push_back(pt);
00185     pt.x = -2;
00186     pt.y = -2;
00187     footprint_spec.push_back(pt);
00188     pt.x = -2;
00189     pt.y = 2;
00190     footprint_spec.push_back(pt);
00191 
00192     tct = new base_local_planner::TrajectoryPlannerTest(mg, wa, map, footprint_spec);
00193   }
00194   return tct;
00195 }
00196 
00197 //make sure that trajectories that intersect obstacles are invalidated
00198 TEST(TrajectoryPlannerTest, footprintObstacles){
00199   TrajectoryPlannerTest* tct = setup_testclass_singleton();
00200   tct->footprintObstacles();
00201 }
00202 
00203 //make sure that goal distance is being computed as expected
00204 TEST(TrajectoryPlannerTest, checkGoalDistance){
00205   TrajectoryPlannerTest* tct = setup_testclass_singleton();
00206   tct->checkGoalDistance();
00207 }
00208 
00209 //make sure that path distance is being computed as expected
00210 TEST(TrajectoryPlannerTest, checkPathDistance){
00211   TrajectoryPlannerTest* tct = setup_testclass_singleton();
00212   tct->checkPathDistance();
00213 }
00214 
00215 }; //namespace


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:08