map_grid_test.cpp
Go to the documentation of this file.
1 /*
2  * map_grid_test.cpp
3  *
4  * Created on: May 2, 2012
5  * Author: tkruse
6  */
7 #include <queue>
8 
9 #include <gtest/gtest.h>
10 
13 
14 #include "wavefront_map_accessor.h"
15 
16 namespace base_local_planner {
17 
18 TEST(MapGridTest, initNull){
19  MapGrid map_grid;
20  EXPECT_EQ(0, map_grid.size_x_);
21  EXPECT_EQ(0, map_grid.size_y_);
22 }
23 
24 TEST(MapGridTest, operatorBrackets){
25  MapGrid map_grid(10, 10);
26  map_grid(3, 5).target_dist = 5;
27  EXPECT_EQ(5, map_grid.getCell(3, 5).target_dist);
28 }
29 
30 TEST(MapGridTest, copyConstructor){
31  MapGrid map_grid(10, 10);
32  map_grid(3, 5).target_dist = 5;
33  MapGrid map_grid2;
34  map_grid2 = map_grid;
35  EXPECT_EQ(5, map_grid(3, 5).target_dist);
36 }
37 
38 TEST(MapGridTest, getIndex){
39  MapGrid map_grid(10, 10);
40  EXPECT_EQ(53, map_grid.getIndex(3, 5));
41 }
42 
43 TEST(MapGridTest, reset){
44  MapGrid map_grid(10, 10);
45  map_grid(0, 0).target_dist = 1;
46  map_grid(0, 0).target_mark = true;
47  map_grid(0, 0).within_robot = true;
48  map_grid(3, 5).target_dist = 1;
49  map_grid(3, 5).target_mark = true;
50  map_grid(3, 5).within_robot = true;
51  map_grid(9, 9).target_dist = 1;
52  map_grid(9, 9).target_mark = true;
53  map_grid(9, 9).within_robot = true;
54  EXPECT_EQ(1, map_grid(0, 0).target_dist);
55  EXPECT_EQ(true, map_grid(0, 0).target_mark);
56  EXPECT_EQ(true, map_grid(0, 0).within_robot);
57  EXPECT_EQ(1, map_grid(3, 5).target_dist);
58  EXPECT_EQ(true, map_grid(3, 5).target_mark);
59  EXPECT_EQ(true, map_grid(3, 5).within_robot);
60  EXPECT_EQ(1, map_grid(9, 9).target_dist);
61  EXPECT_EQ(true, map_grid(9, 9).target_mark);
62  EXPECT_EQ(true, map_grid(9, 9).within_robot);
63 
64  map_grid.resetPathDist();
65 
66  EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(9, 9).target_dist);
67  EXPECT_EQ(false, map_grid(9, 9).target_mark);
68  EXPECT_EQ(false, map_grid(9, 9).within_robot);
69  EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(3, 5).target_dist);
70  EXPECT_EQ(false, map_grid(3, 5).target_mark);
71  EXPECT_EQ(false, map_grid(3, 5).within_robot);
72  EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(0, 0).target_dist);
73  EXPECT_EQ(false, map_grid(0, 0).target_mark);
74  EXPECT_EQ(false, map_grid(0, 0).within_robot);
75 }
76 
77 
78 TEST(MapGridTest, properGridConstruction){
79  MapGrid mg(10, 10);
80  MapCell mc;
81 
82  for(int i = 0; i < 10; ++i){
83  for(int j = 0; j < 10; ++j){
84  EXPECT_FLOAT_EQ(mg(i, j).cx, i);
85  EXPECT_FLOAT_EQ(mg(i, j).cy, j);
86  }
87  }
88 }
89 
90 TEST(MapGridTest, sizeCheck){
91  MapGrid mg(10, 10);
92  MapCell mc;
93 
94  mg.sizeCheck(20, 25);
95 
96  for(int i = 0; i < 20; ++i){
97  for(int j = 0; j < 25; ++j){
98  EXPECT_FLOAT_EQ(mg(i, j).cx, i);
99  EXPECT_FLOAT_EQ(mg(i, j).cy, j);
100  }
101  }
102 }
103 
104 TEST(MapGridTest, adjustPlanEmpty){
105  MapGrid mg(10, 10);
106  const std::vector<geometry_msgs::PoseStamped> global_plan_in;
107  std::vector<geometry_msgs::PoseStamped> global_plan_out;
108  double resolution = 0;
109  mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
110  EXPECT_EQ(0, global_plan_out.size());
111 }
112 
113 TEST(MapGridTest, adjustPlan){
114  MapGrid mg(10, 10);
115  std::vector<geometry_msgs::PoseStamped> global_plan_in;
116  std::vector<geometry_msgs::PoseStamped> global_plan_out;
117  double resolution = 1;
118  geometry_msgs::PoseStamped start;
119  start.pose.position.x = 1;
120  start.pose.position.y = 1;
121  geometry_msgs::PoseStamped end;
122  end.pose.position.x = 5;
123  end.pose.position.y = 5;
124  global_plan_in.push_back(start);
125  global_plan_in.push_back(end);
126  mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
127 
128  EXPECT_EQ(1, global_plan_out[0].pose.position.x);
129  EXPECT_EQ(1, global_plan_out[0].pose.position.y);
130  EXPECT_EQ(5, global_plan_out.back().pose.position.x);
131  EXPECT_EQ(5, global_plan_out.back().pose.position.y);
132 
133  for (unsigned int i = 1; i < global_plan_out.size(); ++i)
134  {
135  geometry_msgs::Point& p0 = global_plan_out[i - 1].pose.position;
136  geometry_msgs::Point& p1 = global_plan_out[i].pose.position;
137  double d = hypot(p0.x - p1.x, p0.y - p1.y);
138  EXPECT_LT(d, resolution);
139  }
140 }
141 
142 TEST(MapGridTest, adjustPlan2){
143  std::vector<geometry_msgs::PoseStamped> base_plan, result;
144 
145  // Push two points, at (0,0) and (0,1). Gap is 1 meter
146  base_plan.push_back(geometry_msgs::PoseStamped());
147  base_plan.push_back(geometry_msgs::PoseStamped());
148  base_plan.back().pose.position.y = 1.0;
149 
150  // resolution >= 1, path won't change
151  MapGrid::adjustPlanResolution(base_plan, result, 2.0);
152  EXPECT_EQ(2, result.size());
153  result.clear();
154  MapGrid::adjustPlanResolution(base_plan, result, 1.0);
155  EXPECT_EQ(2, result.size());
156  result.clear();
157 
158  // 0.5 <= resolution < 1.0, one point should be added in the middle
159  MapGrid::adjustPlanResolution(base_plan, result, 0.8);
160  EXPECT_EQ(3, result.size());
161  result.clear();
162  MapGrid::adjustPlanResolution(base_plan, result, 0.5);
163  EXPECT_EQ(3, result.size());
164  result.clear();
165 
166  // 0.333 <= resolution < 0.5, two points should be added in the middle
167  MapGrid::adjustPlanResolution(base_plan, result, 0.34);
168  EXPECT_EQ(4, result.size());
169  result.clear();
170 
171  // 0.25 <= resolution < 0.333, three points should be added in the middle
172  MapGrid::adjustPlanResolution(base_plan, result, 0.32);
173  EXPECT_EQ(5, result.size());
174  result.clear();
175 
176  MapGrid::adjustPlanResolution(base_plan, result, 0.1);
177  EXPECT_EQ(11, result.size());
178  result.clear();
179 }
180 
181 TEST(MapGridTest, distancePropagation){
182  MapGrid mg(10, 10);
183 
184  WavefrontMapAccessor* wa = new WavefrontMapAccessor(&mg, .25);
185  std::queue<MapCell*> dist_queue;
186  mg.computeTargetDistance(dist_queue, *wa);
187  EXPECT_EQ(false, mg(0, 0).target_mark);
188 
189  MapCell& mc = mg.getCell(0, 0);
190  mc.target_dist = 0.0;
191  mc.target_mark = true;
192  dist_queue.push(&mc);
193  mg.computeTargetDistance(dist_queue, *wa);
194  EXPECT_EQ(true, mg(0, 0).target_mark);
195  EXPECT_EQ(0.0, mg(0, 0).target_dist);
196  EXPECT_EQ(true, mg(1, 1).target_mark);
197  EXPECT_EQ(2.0, mg(1, 1).target_dist);
198  EXPECT_EQ(true, mg(0, 4).target_mark);
199  EXPECT_EQ(4.0, mg(0, 4).target_dist);
200  EXPECT_EQ(true, mg(4, 0).target_mark);
201  EXPECT_EQ(4.0, mg(4, 0).target_dist);
202  EXPECT_EQ(true, mg(9, 9).target_mark);
203  EXPECT_EQ(18.0, mg(9, 9).target_dist);
204 }
205 
206 }
d
Definition: setup.py:5
void computeTargetDistance(std::queue< MapCell * > &dist_queue, const costmap_2d::Costmap2D &costmap)
Compute the distance from each cell in the local map grid to the planned path.
Definition: map_grid.cpp:255
unsigned int size_y_
The dimensions of the grid.
Definition: map_grid.h:191
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
Definition: map_grid.h:52
static void adjustPlanResolution(const std::vector< geometry_msgs::PoseStamped > &global_plan_in, std::vector< geometry_msgs::PoseStamped > &global_plan_out, double resolution)
Definition: map_grid.cpp:133
size_t getIndex(int x, int y)
Returns a 1D index into the MapCell array for a 2D index.
Definition: map_grid.cpp:73
void sizeCheck(unsigned int size_x, unsigned int size_y)
check if we need to resize
Definition: map_grid.cpp:84
bool target_mark
Marks for computing path/goal distances.
Definition: map_cell.h:61
double target_dist
Distance to planner&#39;s path.
Definition: map_cell.h:59
Stores path distance and goal distance information used for scoring trajectories. ...
Definition: map_cell.h:44
MapCell & getCell(unsigned int x, unsigned int y)
Definition: map_grid.h:87
TEST(FootprintHelperTest, correctFootprint)
void resetPathDist()
reset path distance fields for all cells
Definition: map_grid.cpp:125


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