footprint_helper_test.cpp
Go to the documentation of this file.
00001 /*
00002  * footprint_helper_test.cpp
00003  *
00004  *  Created on: May 2, 2012
00005  *      Author: tkruse
00006  */
00007 
00008 #include <gtest/gtest.h>
00009 
00010 #include <vector>
00011 
00012 #include <base_local_planner/footprint_helper.h>
00013 
00014 #include <base_local_planner/map_grid.h>
00015 #include <base_local_planner/costmap_model.h>
00016 #include <costmap_2d/costmap_2d.h>
00017 
00018 #include "wavefront_map_accessor.h"
00019 
00020 namespace base_local_planner {
00021 
00022 
00023 class FootprintHelperTest : public testing::Test {
00024 public:
00025   FootprintHelper fh;
00026 
00027   FootprintHelperTest() {
00028   }
00029 
00030   virtual void TestBody(){}
00031 
00032   void correctLineCells() {
00033     std::vector<base_local_planner::Position2DInt> footprint;
00034     fh.getLineCells(0, 10, 0, 10, footprint);
00035     EXPECT_EQ(11, footprint.size());
00036     EXPECT_EQ(footprint[0].x, 0);
00037     EXPECT_EQ(footprint[0].y, 0);
00038     EXPECT_EQ(footprint[5].x, 5);
00039     EXPECT_EQ(footprint[5].y, 5);
00040     EXPECT_EQ(footprint[10].x, 10);
00041     EXPECT_EQ(footprint[10].y, 10);
00042   }
00043 
00044   void correctFootprint(){
00045     MapGrid* mg = new MapGrid (10, 10);
00046     WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
00047     const costmap_2d::Costmap2D& map = *wa;
00048 
00049     std::vector<geometry_msgs::Point> footprint_spec;
00050     geometry_msgs::Point pt;
00051     //create a square footprint
00052     pt.x = 2;
00053     pt.y = 2;
00054     footprint_spec.push_back(pt);
00055     pt.x = 2;
00056     pt.y = -2;
00057     footprint_spec.push_back(pt);
00058     pt.x = -2;
00059     pt.y = -2;
00060     footprint_spec.push_back(pt);
00061     pt.x = -2;
00062     pt.y = 2;
00063     footprint_spec.push_back(pt);
00064 
00065     Eigen::Vector3f pos(4.5, 4.5, 0);
00066     //just create a basic footprint
00067     std::vector<base_local_planner::Position2DInt> footprint = fh.getFootprintCells(pos, footprint_spec, map, false);
00068 
00069     EXPECT_EQ(20, footprint.size());
00070     //we expect the front line to be first
00071     EXPECT_EQ(footprint[0].x, 6); EXPECT_EQ(footprint[0].y, 6);
00072     EXPECT_EQ(footprint[1].x, 6); EXPECT_EQ(footprint[1].y, 5);
00073     EXPECT_EQ(footprint[2].x, 6); EXPECT_EQ(footprint[2].y, 4);
00074     EXPECT_EQ(footprint[3].x, 6); EXPECT_EQ(footprint[3].y, 3);
00075     EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 2);
00076 
00077     //next the right line
00078     EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 2);
00079     EXPECT_EQ(footprint[6].x, 5); EXPECT_EQ(footprint[6].y, 2);
00080     EXPECT_EQ(footprint[7].x, 4); EXPECT_EQ(footprint[7].y, 2);
00081     EXPECT_EQ(footprint[8].x, 3); EXPECT_EQ(footprint[8].y, 2);
00082     EXPECT_EQ(footprint[9].x, 2); EXPECT_EQ(footprint[9].y, 2);
00083 
00084     //next the back line
00085     EXPECT_EQ(footprint[10].x, 2); EXPECT_EQ(footprint[10].y, 2);
00086     EXPECT_EQ(footprint[11].x, 2); EXPECT_EQ(footprint[11].y, 3);
00087     EXPECT_EQ(footprint[12].x, 2); EXPECT_EQ(footprint[12].y, 4);
00088     EXPECT_EQ(footprint[13].x, 2); EXPECT_EQ(footprint[13].y, 5);
00089     EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 6);
00090 
00091     //finally the left line
00092     EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 6);
00093     EXPECT_EQ(footprint[16].x, 3); EXPECT_EQ(footprint[16].y, 6);
00094     EXPECT_EQ(footprint[17].x, 4); EXPECT_EQ(footprint[17].y, 6);
00095     EXPECT_EQ(footprint[18].x, 5); EXPECT_EQ(footprint[18].y, 6);
00096     EXPECT_EQ(footprint[19].x, 6); EXPECT_EQ(footprint[19].y, 6);
00097 
00098 
00099     pos = Eigen::Vector3f(4.5, 4.5, M_PI_2);
00100     //check that rotation of the footprint works
00101     footprint = fh.getFootprintCells(pos, footprint_spec, map, false);
00102 
00103     //first the left line
00104     EXPECT_EQ(footprint[0].x, 2); EXPECT_EQ(footprint[0].y, 6);
00105     EXPECT_EQ(footprint[1].x, 3); EXPECT_EQ(footprint[1].y, 6);
00106     EXPECT_EQ(footprint[2].x, 4); EXPECT_EQ(footprint[2].y, 6);
00107     EXPECT_EQ(footprint[3].x, 5); EXPECT_EQ(footprint[3].y, 6);
00108     EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 6);
00109 
00110     //next the front line
00111     EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 6);
00112     EXPECT_EQ(footprint[6].x, 6); EXPECT_EQ(footprint[6].y, 5);
00113     EXPECT_EQ(footprint[7].x, 6); EXPECT_EQ(footprint[7].y, 4);
00114     EXPECT_EQ(footprint[8].x, 6); EXPECT_EQ(footprint[8].y, 3);
00115     EXPECT_EQ(footprint[9].x, 6); EXPECT_EQ(footprint[9].y, 2);
00116 
00117     //next the right line
00118     EXPECT_EQ(footprint[10].x, 6); EXPECT_EQ(footprint[10].y, 2);
00119     EXPECT_EQ(footprint[11].x, 5); EXPECT_EQ(footprint[11].y, 2);
00120     EXPECT_EQ(footprint[12].x, 4); EXPECT_EQ(footprint[12].y, 2);
00121     EXPECT_EQ(footprint[13].x, 3); EXPECT_EQ(footprint[13].y, 2);
00122     EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 2);
00123 
00124     //next the back line
00125     EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 2);
00126     EXPECT_EQ(footprint[16].x, 2); EXPECT_EQ(footprint[16].y, 3);
00127     EXPECT_EQ(footprint[17].x, 2); EXPECT_EQ(footprint[17].y, 4);
00128     EXPECT_EQ(footprint[18].x, 2); EXPECT_EQ(footprint[18].y, 5);
00129     EXPECT_EQ(footprint[19].x, 2); EXPECT_EQ(footprint[19].y, 6);
00130   }
00131 
00132 };
00133 
00134 
00135 TEST(FootprintHelperTest, correctFootprint){
00136   FootprintHelperTest tct;
00137   tct.correctFootprint();
00138 }
00139 
00140 TEST(FootprintHelperTest, correctLineCells){
00141   FootprintHelperTest tct;
00142   tct.correctLineCells();
00143 }
00144 
00145 }


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