footprint_helper_test.cpp
Go to the documentation of this file.
1 /*
2  * footprint_helper_test.cpp
3  *
4  * Created on: May 2, 2012
5  * Author: tkruse
6  */
7 
8 #include <gtest/gtest.h>
9 
10 #include <vector>
11 
13 
16 #include <costmap_2d/costmap_2d.h>
17 
18 #include "wavefront_map_accessor.h"
19 
20 namespace base_local_planner {
21 
22 
23 class FootprintHelperTest : public testing::Test {
24 public:
26 
28  }
29 
30  virtual void TestBody(){}
31 
33  std::vector<base_local_planner::Position2DInt> footprint;
34  fh.getLineCells(0, 10, 0, 10, footprint);
35  EXPECT_EQ(11, footprint.size());
36  EXPECT_EQ(footprint[0].x, 0);
37  EXPECT_EQ(footprint[0].y, 0);
38  EXPECT_EQ(footprint[5].x, 5);
39  EXPECT_EQ(footprint[5].y, 5);
40  EXPECT_EQ(footprint[10].x, 10);
41  EXPECT_EQ(footprint[10].y, 10);
42  }
43 
45  MapGrid* mg = new MapGrid (10, 10);
46  WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
47  const costmap_2d::Costmap2D& map = *wa;
48 
49  std::vector<geometry_msgs::Point> footprint_spec;
50  geometry_msgs::Point pt;
51  //create a square footprint
52  pt.x = 2;
53  pt.y = 2;
54  footprint_spec.push_back(pt);
55  pt.x = 2;
56  pt.y = -2;
57  footprint_spec.push_back(pt);
58  pt.x = -2;
59  pt.y = -2;
60  footprint_spec.push_back(pt);
61  pt.x = -2;
62  pt.y = 2;
63  footprint_spec.push_back(pt);
64 
65  Eigen::Vector3f pos(4.5, 4.5, 0);
66  //just create a basic footprint
67  std::vector<base_local_planner::Position2DInt> footprint = fh.getFootprintCells(pos, footprint_spec, map, false);
68 
69  EXPECT_EQ(20, footprint.size());
70  //we expect the front line to be first
71  EXPECT_EQ(footprint[0].x, 6); EXPECT_EQ(footprint[0].y, 6);
72  EXPECT_EQ(footprint[1].x, 6); EXPECT_EQ(footprint[1].y, 5);
73  EXPECT_EQ(footprint[2].x, 6); EXPECT_EQ(footprint[2].y, 4);
74  EXPECT_EQ(footprint[3].x, 6); EXPECT_EQ(footprint[3].y, 3);
75  EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 2);
76 
77  //next the right line
78  EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 2);
79  EXPECT_EQ(footprint[6].x, 5); EXPECT_EQ(footprint[6].y, 2);
80  EXPECT_EQ(footprint[7].x, 4); EXPECT_EQ(footprint[7].y, 2);
81  EXPECT_EQ(footprint[8].x, 3); EXPECT_EQ(footprint[8].y, 2);
82  EXPECT_EQ(footprint[9].x, 2); EXPECT_EQ(footprint[9].y, 2);
83 
84  //next the back line
85  EXPECT_EQ(footprint[10].x, 2); EXPECT_EQ(footprint[10].y, 2);
86  EXPECT_EQ(footprint[11].x, 2); EXPECT_EQ(footprint[11].y, 3);
87  EXPECT_EQ(footprint[12].x, 2); EXPECT_EQ(footprint[12].y, 4);
88  EXPECT_EQ(footprint[13].x, 2); EXPECT_EQ(footprint[13].y, 5);
89  EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 6);
90 
91  //finally the left line
92  EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 6);
93  EXPECT_EQ(footprint[16].x, 3); EXPECT_EQ(footprint[16].y, 6);
94  EXPECT_EQ(footprint[17].x, 4); EXPECT_EQ(footprint[17].y, 6);
95  EXPECT_EQ(footprint[18].x, 5); EXPECT_EQ(footprint[18].y, 6);
96  EXPECT_EQ(footprint[19].x, 6); EXPECT_EQ(footprint[19].y, 6);
97 
98 
99  pos = Eigen::Vector3f(4.5, 4.5, M_PI_2);
100  //check that rotation of the footprint works
101  footprint = fh.getFootprintCells(pos, footprint_spec, map, false);
102 
103  //first the left line
104  EXPECT_EQ(footprint[0].x, 2); EXPECT_EQ(footprint[0].y, 6);
105  EXPECT_EQ(footprint[1].x, 3); EXPECT_EQ(footprint[1].y, 6);
106  EXPECT_EQ(footprint[2].x, 4); EXPECT_EQ(footprint[2].y, 6);
107  EXPECT_EQ(footprint[3].x, 5); EXPECT_EQ(footprint[3].y, 6);
108  EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 6);
109 
110  //next the front line
111  EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 6);
112  EXPECT_EQ(footprint[6].x, 6); EXPECT_EQ(footprint[6].y, 5);
113  EXPECT_EQ(footprint[7].x, 6); EXPECT_EQ(footprint[7].y, 4);
114  EXPECT_EQ(footprint[8].x, 6); EXPECT_EQ(footprint[8].y, 3);
115  EXPECT_EQ(footprint[9].x, 6); EXPECT_EQ(footprint[9].y, 2);
116 
117  //next the right line
118  EXPECT_EQ(footprint[10].x, 6); EXPECT_EQ(footprint[10].y, 2);
119  EXPECT_EQ(footprint[11].x, 5); EXPECT_EQ(footprint[11].y, 2);
120  EXPECT_EQ(footprint[12].x, 4); EXPECT_EQ(footprint[12].y, 2);
121  EXPECT_EQ(footprint[13].x, 3); EXPECT_EQ(footprint[13].y, 2);
122  EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 2);
123 
124  //next the back line
125  EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 2);
126  EXPECT_EQ(footprint[16].x, 2); EXPECT_EQ(footprint[16].y, 3);
127  EXPECT_EQ(footprint[17].x, 2); EXPECT_EQ(footprint[17].y, 4);
128  EXPECT_EQ(footprint[18].x, 2); EXPECT_EQ(footprint[18].y, 5);
129  EXPECT_EQ(footprint[19].x, 2); EXPECT_EQ(footprint[19].y, 6);
130  }
131 
132 };
133 
134 
137  tct.correctFootprint();
138 }
139 
142  tct.correctLineCells();
143 }
144 
145 }
std::vector< base_local_planner::Position2DInt > getFootprintCells(Eigen::Vector3f pos, std::vector< geometry_msgs::Point > footprint_spec, const costmap_2d::Costmap2D &, bool fill)
Used to get the cells that make up the footprint of the robot.
TrajectoryPlannerTest * tct
Definition: utest.cpp:169
TFSIMD_FORCE_INLINE const tfScalar & y() const
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
Definition: map_grid.h:52
TFSIMD_FORCE_INLINE const tfScalar & x() const
void getLineCells(int x0, int x1, int y0, int y1, std::vector< base_local_planner::Position2DInt > &pts)
Use Bresenham&#39;s algorithm to trace a line between two points in a grid.
TEST(FootprintHelperTest, correctFootprint)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49