00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
00084 tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
00085
00086 EXPECT_FLOAT_EQ(traj.cost_, -1.0);
00087
00088
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
00098
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
00101 EXPECT_FLOAT_EQ(traj.cost_, -1.0);
00102 }
00103
00104 void TrajectoryPlannerTest::checkGoalDistance(){
00105
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
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(¤t);
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);
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
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(¤t);
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);
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
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
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
00198 TEST(TrajectoryPlannerTest, footprintObstacles){
00199 TrajectoryPlannerTest* tct = setup_testclass_singleton();
00200 tct->footprintObstacles();
00201 }
00202
00203
00204 TEST(TrajectoryPlannerTest, checkGoalDistance){
00205 TrajectoryPlannerTest* tct = setup_testclass_singleton();
00206 tct->checkGoalDistance();
00207 }
00208
00209
00210 TEST(TrajectoryPlannerTest, checkPathDistance){
00211 TrajectoryPlannerTest* tct = setup_testclass_singleton();
00212 tct->checkPathDistance();
00213 }
00214
00215 };