00001
00002
00003
00004
00005
00006
00007 #include <queue>
00008
00009 #include <gtest/gtest.h>
00010
00011 #include <base_local_planner/map_grid.h>
00012 #include <base_local_planner/map_cell.h>
00013
00014 #include "wavefront_map_accessor.h"
00015
00016 namespace base_local_planner {
00017
00018 TEST(MapGridTest, initNull){
00019 MapGrid map_grid;
00020 EXPECT_EQ(0, map_grid.size_x_);
00021 EXPECT_EQ(0, map_grid.size_y_);
00022 }
00023
00024 TEST(MapGridTest, operatorBrackets){
00025 MapGrid map_grid(10, 10);
00026 map_grid(3, 5).target_dist = 5;
00027 EXPECT_EQ(5, map_grid.getCell(3, 5).target_dist);
00028 }
00029
00030 TEST(MapGridTest, copyConstructor){
00031 MapGrid map_grid(10, 10);
00032 map_grid(3, 5).target_dist = 5;
00033 MapGrid map_grid2;
00034 map_grid2 = map_grid;
00035 EXPECT_EQ(5, map_grid(3, 5).target_dist);
00036 }
00037
00038 TEST(MapGridTest, getIndex){
00039 MapGrid map_grid(10, 10);
00040 EXPECT_EQ(53, map_grid.getIndex(3, 5));
00041 }
00042
00043 TEST(MapGridTest, reset){
00044 MapGrid map_grid(10, 10);
00045 map_grid(0, 0).target_dist = 1;
00046 map_grid(0, 0).target_mark = true;
00047 map_grid(0, 0).within_robot = true;
00048 map_grid(3, 5).target_dist = 1;
00049 map_grid(3, 5).target_mark = true;
00050 map_grid(3, 5).within_robot = true;
00051 map_grid(9, 9).target_dist = 1;
00052 map_grid(9, 9).target_mark = true;
00053 map_grid(9, 9).within_robot = true;
00054 EXPECT_EQ(1, map_grid(0, 0).target_dist);
00055 EXPECT_EQ(true, map_grid(0, 0).target_mark);
00056 EXPECT_EQ(true, map_grid(0, 0).within_robot);
00057 EXPECT_EQ(1, map_grid(3, 5).target_dist);
00058 EXPECT_EQ(true, map_grid(3, 5).target_mark);
00059 EXPECT_EQ(true, map_grid(3, 5).within_robot);
00060 EXPECT_EQ(1, map_grid(9, 9).target_dist);
00061 EXPECT_EQ(true, map_grid(9, 9).target_mark);
00062 EXPECT_EQ(true, map_grid(9, 9).within_robot);
00063
00064 map_grid.resetPathDist();
00065
00066 EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(9, 9).target_dist);
00067 EXPECT_EQ(false, map_grid(9, 9).target_mark);
00068 EXPECT_EQ(false, map_grid(9, 9).within_robot);
00069 EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(3, 5).target_dist);
00070 EXPECT_EQ(false, map_grid(3, 5).target_mark);
00071 EXPECT_EQ(false, map_grid(3, 5).within_robot);
00072 EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(0, 0).target_dist);
00073 EXPECT_EQ(false, map_grid(0, 0).target_mark);
00074 EXPECT_EQ(false, map_grid(0, 0).within_robot);
00075 }
00076
00077
00078 TEST(MapGridTest, properGridConstruction){
00079 MapGrid mg(10, 10);
00080 MapCell mc;
00081
00082 for(int i = 0; i < 10; ++i){
00083 for(int j = 0; j < 10; ++j){
00084 EXPECT_FLOAT_EQ(mg(i, j).cx, i);
00085 EXPECT_FLOAT_EQ(mg(i, j).cy, j);
00086 }
00087 }
00088 }
00089
00090 TEST(MapGridTest, sizeCheck){
00091 MapGrid mg(10, 10);
00092 MapCell mc;
00093
00094 mg.sizeCheck(20, 25);
00095
00096 for(int i = 0; i < 20; ++i){
00097 for(int j = 0; j < 25; ++j){
00098 EXPECT_FLOAT_EQ(mg(i, j).cx, i);
00099 EXPECT_FLOAT_EQ(mg(i, j).cy, j);
00100 }
00101 }
00102 }
00103
00104 TEST(MapGridTest, adjustPlanEmpty){
00105 MapGrid mg(10, 10);
00106 const std::vector<geometry_msgs::PoseStamped> global_plan_in;
00107 std::vector<geometry_msgs::PoseStamped> global_plan_out;
00108 double resolution = 0;
00109 mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
00110 EXPECT_EQ(0, global_plan_out.size());
00111 }
00112
00113 TEST(MapGridTest, adjustPlan){
00114 MapGrid mg(10, 10);
00115 std::vector<geometry_msgs::PoseStamped> global_plan_in;
00116 std::vector<geometry_msgs::PoseStamped> global_plan_out;
00117 double resolution = 1;
00118 geometry_msgs::PoseStamped start;
00119 start.pose.position.x = 1;
00120 start.pose.position.y = 1;
00121 geometry_msgs::PoseStamped end;
00122 end.pose.position.x = 5;
00123 end.pose.position.y = 5;
00124 global_plan_in.push_back(start);
00125 global_plan_in.push_back(end);
00126 mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
00127 EXPECT_EQ(3, global_plan_out.size());
00128
00129 EXPECT_EQ(1, global_plan_out[0].pose.position.x);
00130 EXPECT_EQ(1, global_plan_out[0].pose.position.x);
00131 EXPECT_EQ(3, global_plan_out[1].pose.position.x);
00132 EXPECT_EQ(3, global_plan_out[1].pose.position.x);
00133 EXPECT_EQ(5, global_plan_out[2].pose.position.x);
00134 EXPECT_EQ(5, global_plan_out[2].pose.position.x);
00135 }
00136
00137 TEST(MapGridTest, distancePropagation){
00138 MapGrid mg(10, 10);
00139
00140 WavefrontMapAccessor* wa = new WavefrontMapAccessor(&mg, .25);
00141 std::queue<MapCell*> dist_queue;
00142 mg.computeTargetDistance(dist_queue, *wa);
00143 EXPECT_EQ(false, mg(0, 0).target_mark);
00144
00145 MapCell& mc = mg.getCell(0, 0);
00146 mc.target_dist = 0.0;
00147 mc.target_mark = true;
00148 dist_queue.push(&mc);
00149 mg.computeTargetDistance(dist_queue, *wa);
00150 EXPECT_EQ(true, mg(0, 0).target_mark);
00151 EXPECT_EQ(0.0, mg(0, 0).target_dist);
00152 EXPECT_EQ(true, mg(1, 1).target_mark);
00153 EXPECT_EQ(2.0, mg(1, 1).target_dist);
00154 EXPECT_EQ(true, mg(0, 4).target_mark);
00155 EXPECT_EQ(4.0, mg(0, 4).target_dist);
00156 EXPECT_EQ(true, mg(4, 0).target_mark);
00157 EXPECT_EQ(4.0, mg(4, 0).target_dist);
00158 EXPECT_EQ(true, mg(9, 9).target_mark);
00159 EXPECT_EQ(18.0, mg(9, 9).target_dist);
00160 }
00161
00162 }