map_grid_test.cpp
Go to the documentation of this file.
00001 /*
00002  * map_grid_test.cpp
00003  *
00004  *  Created on: May 2, 2012
00005  *      Author: tkruse
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 }


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