utest.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 #include <gtest/gtest.h>
36 #include <algorithm>
37 #include <vector>
38 
39 using nav_grid::Index;
40 
41 template<class iterator_type>
42 int countIterations(iterator_type it, int max_iterations = 1000)
43 {
44  int count = 0;
45  iterator_type end = it.end();
46  for ( ; it != end; ++it)
47  {
48  ++count;
49  if (count >= max_iterations) break;
50  }
51  return count;
52 }
53 
54 TEST(WholeGrid, whole_grid)
55 {
57  info.width = 5;
58  info.height = 8;
59  int count = 0;
60  for (nav_grid_iterators::WholeGrid it(info); it != it.end(); ++it)
61  {
62  Index i = *it;
63  ASSERT_EQ(i.x, count % info.width);
64  ASSERT_EQ(i.y, count / info.width);
65  ++count;
66  }
67  ASSERT_EQ(count, 40);
68 }
69 
70 TEST(WholeGrid, whole_grid_range)
71 {
73  info.width = 3;
74  info.height = 6;
75  int count = 0;
76  for (Index i : nav_grid_iterators::WholeGrid(info))
77  {
78  ASSERT_EQ(i.x, count % info.width);
79  ASSERT_EQ(i.y, count / info.width);
80  ++count;
81  }
82  ASSERT_EQ(count, 18);
83 }
84 
85 TEST(WholeGrid, std_stuff)
86 {
88  info.width = 8;
89  info.height = 2;
91 
92  std::vector<Index> vec;
93  std::copy(wg.begin(), wg.end(), std::back_inserter(vec));
94  for (int count = 0; count < 16; ++count)
95  {
96  Index& i = vec[count];
97  ASSERT_EQ(i.x, count % info.width);
98  ASSERT_EQ(i.y, count / info.width);
99  }
100 }
101 
102 TEST(SubGrid, sub_grid)
103 {
105  info.width = 5;
106  info.height = 8;
107  int count = 0;
108  for (Index i : nav_grid_iterators::SubGrid(&info, 1, 2, 2, 3))
109  {
110  ASSERT_EQ(i.x, static_cast<unsigned int>(1 + count % 2));
111  ASSERT_EQ(i.y, static_cast<unsigned int>(2 + count / 2));
112  ++count;
113  }
114  ASSERT_EQ(count, 6);
115 
116  ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, 1, 3, 4, 1)), 4);
117  ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, 1, 3, 2, 2)), 4);
118  nav_core2::UIntBounds bounds(1, 3, 4, 3);
119  ASSERT_EQ(countIterations(nav_grid_iterators::SubGrid(&info, bounds)), 4);
120 }
121 
122 TEST(SubGrid, equality)
123 {
125  info.width = 5;
126  info.height = 8;
127  nav_grid_iterators::SubGrid it1(&info, 1, 2, 2, 3);
128  nav_grid_iterators::SubGrid it2(&info, 1, 2, 1, 1);
129  ASSERT_FALSE(it1 == it2);
130 }
131 
132 TEST(CircleFill, circle)
133 {
135  info.width = 8;
136  info.height = 8;
137  info.resolution = 1.0;
138 
139  ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 4.0, 4.0, 3.0)), 32);
140  ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 4.3, 4.0, 3.0)), 28);
141  ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 4.0, 4.0, 8.0)), 64);
142  ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 14.0, 4.0, 1.0)), 0);
143  ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 0.0, 4.0, 4.0)), 26);
144  ASSERT_EQ(countIterations(nav_grid_iterators::CircleFill(&info, 4.0, 4.0, 3.0).begin()), 32);
145 }
146 
147 TEST(CircleFill, equality)
148 {
150  info.width = 5;
151  info.height = 8;
152  nav_grid_iterators::CircleFill it1(&info, 4.0, 4.0, 8.0);
153  nav_grid_iterators::CircleFill it2(&info, 1.0, 1.0, 100.0);
154  ASSERT_FALSE(it1 == it2);
155 }
156 
157 TEST(CircleOutline, circle_outline)
158 {
160  info.width = 8;
161  info.height = 8;
162  info.resolution = 1.0;
163 
164  unsigned int size = 0;
165  EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 1);
166  EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 8);
167  EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 16);
168  EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 20);
169  EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 14);
170  EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 5);
171  EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 0);
172  EXPECT_EQ(countIterations(nav_grid_iterators::CircleOutline(&info, 4.0, 4.0, size++)), 0);
173 }
174 
175 TEST(CircleOutline, equality)
176 {
178  info.width = 5;
179  info.height = 8;
180  nav_grid_iterators::CircleOutline it1(&info, 3.0, 1.0, 1.0);
181  nav_grid_iterators::CircleOutline it2(&info, 1.0, 1.0, 3.0);
182  ASSERT_FALSE(it1 == it2);
183 }
184 
185 TEST(Spiral, spiral)
186 {
188  info.width = 8;
189  info.height = 8;
190  info.resolution = 1.0;
191  info.origin_x = 0.0;
192  info.origin_y = 0.0;
193  ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 4.0, 4.0, 3.0)), 32);
194  ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 4.3, 4.0, 3.0)), 28);
195  ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 4.0, 4.0, 8.0)), 64);
196  ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 14.0, 4.0, 1.0)), 0);
197  ASSERT_EQ(countIterations(nav_grid_iterators::Spiral(&info, 0.0, 4.0, 4.0)), 26);
198 }
199 
200 TEST(Spiral, equality)
201 {
203  info.width = 5;
204  info.height = 8;
205  nav_grid_iterators::Spiral it1(&info, 1.0, 1.0, 1.0);
206  nav_grid_iterators::Spiral it2(&info, 1.0, 1.0, 3.0);
207  ASSERT_FALSE(it1 == it2);
208 }
209 
210 TEST(Line, signed_line)
211 {
213  info.width = 10;
214  info.height = 10;
215  info.resolution = 1.0;
216  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, 0)), 1);
217  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 3, 0)), 4);
218  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, -3, 0)), 1);
219  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 3, 0, 0, 0)), 4);
220  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -3, 0, 0, 0)), 1);
221  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -3, 0, 0, 0, false)), 0);
222  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, 3)), 4);
223  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, -3)), 1);
224  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 3, 0, 0)), 4);
225  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, -3, 0, 0)), 1);
226  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 1, 9)), 10);
227  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 1, 9, 0, 0)), 10);
228  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 9, 1, 0, 0)), 10);
229  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 9, 1)), 10);
230 
231  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -5, 5, 15, 5)), 10);
232  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 15, 5, -5, 5)), 10);
233  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 15, 5, 15, 7)), 0);
234 
235  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -1, -5, -1, -7)), 0);
236 }
237 
238 TEST(Line, signed_line_diff_res)
239 {
240  // This is the same test as above with a reduced resolution. Most of the coordinates are divided by 10
241  // (with an additional 0.05 in some places to avoid floating point errors)
243  info.width = 10;
244  info.height = 10;
245  info.resolution = 0.1;
246  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, 0)), 1);
247  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0.35, 0)), 4);
248  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, -0.35, 0)), 1);
249  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0.35, 0, 0, 0)), 4);
250  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -0.35, 0, 0, 0)), 1);
251  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -0.35, 0, 0, 0, false)), 0);
252  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, 0.35)), 4);
253  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0, -0.35)), 1);
254  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0.35, 0, 0)), 4);
255  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, -0.35, 0, 0)), 1);
256  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0.15, 0.95)), 10);
257  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0.15, 0.95, 0, 0)), 10);
258  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0.95, 0.15, 0, 0)), 10);
259  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 0, 0, 0.95, 0.15)), 10);
260 
261  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -0.5, 0.5, 1.5, 0.5)), 10);
262  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 1.5, 0.5, -0.55, 0.55)), 10);
263  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 1.5, 0.5, 1.5, 0.75)), 0);
264 
265  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, -0.1, -0.5, -0.1, -0.7)), 0);
266 }
267 
268 TEST(Line, random_test_case)
269 {
271  info.width = 795;
272  info.height = 925;
273  info.resolution = 0.05;
274  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 6.2402, 30.651832, 2.805347, 22.8941, false, false)), 224);
275  EXPECT_EQ(countIterations(nav_grid_iterators::Line(&info, 6.2402, 30.651832, 2.805347, 22.8941, true, false)), 225);
276 }
277 
278 TEST(Line, equality)
279 {
281  info.width = 5;
282  info.height = 8;
283  nav_grid_iterators::Line it1(&info, 0, 0, 5, 5);
284  nav_grid_iterators::Line it2(&info, 0, 0, 1, 1);
285  ASSERT_FALSE(it1 == it2);
286 }
287 
288 nav_2d_msgs::Point2D make_point(double x, double y)
289 {
290  nav_2d_msgs::Point2D pt;
291  pt.x = x;
292  pt.y = y;
293  return pt;
294 }
295 
296 TEST(Polygon, polygon)
297 {
299  // First check to make sure it works when the polygon is completely on the grid
300  info.width = 10;
301  info.height = 10;
302  info.resolution = 1.0;
303  nav_2d_msgs::Polygon2D simple_square;
304  simple_square.points.push_back(make_point(1.4, 1.4));
305  simple_square.points.push_back(make_point(1.4, 3.6));
306  simple_square.points.push_back(make_point(3.6, 3.6));
307  simple_square.points.push_back(make_point(3.6, 1.4));
308 
309  EXPECT_EQ(countIterations(nav_grid_iterators::PolygonOutline(&info, simple_square)), 8);
310  EXPECT_EQ(countIterations(nav_grid_iterators::PolygonFill(&info, simple_square)), 9);
311 
312  // Then do it again when it is only partially on the grid
313  info.height = 3;
314  EXPECT_EQ(countIterations(nav_grid_iterators::PolygonOutline(&info, simple_square)), 5);
315  EXPECT_EQ(countIterations(nav_grid_iterators::PolygonFill(&info, simple_square)), 6);
316 
317  // Then check when it is completely off the grid
318  info.resolution = 0.1;
319  EXPECT_EQ(countIterations(nav_grid_iterators::PolygonOutline(&info, simple_square)), 0);
320  EXPECT_EQ(countIterations(nav_grid_iterators::PolygonFill(&info, simple_square)), 0);
321 }
322 
323 TEST(Polygon, empty_polygon)
324 {
326  info.width = 10;
327  info.height = 10;
328  info.resolution = 1.0;
329  nav_2d_msgs::Polygon2D empty_polygon;
330 
331  EXPECT_EQ(countIterations(nav_grid_iterators::PolygonOutline(&info, empty_polygon)), 0);
332  EXPECT_EQ(countIterations(nav_grid_iterators::PolygonFill(&info, empty_polygon)), 0);
333 }
334 
335 TEST(Polygon, equality)
336 {
338  info.width = 10;
339  info.height = 10;
340  info.resolution = 1.0;
341  nav_2d_msgs::Polygon2D simple_square;
342  simple_square.points.push_back(make_point(1.4, 1.4));
343  simple_square.points.push_back(make_point(1.4, 3.6));
344  simple_square.points.push_back(make_point(3.6, 3.6));
345  simple_square.points.push_back(make_point(3.6, 1.4));
346 
347  nav_2d_msgs::Polygon2D triangle;
348  triangle.points.push_back(make_point(1.4, 1.4));
349  triangle.points.push_back(make_point(1.4, 3.6));
350  triangle.points.push_back(make_point(3.6, 3.6));
351 
352 
353  nav_grid_iterators::PolygonOutline it1(&info, simple_square);
354  nav_grid_iterators::PolygonOutline it2(&info, triangle);
355  ASSERT_FALSE(it1 == it2);
356 
357  nav_grid_iterators::PolygonFill it3(&info, simple_square);
358  nav_grid_iterators::PolygonFill it4(&info, triangle);
359  ASSERT_FALSE(it3 == it4);
360 }
361 
362 TEST(Iterators, test_copy)
363 {
364  // This test will fail to compile if you cannot use the copy operator
366  info.width = 10;
367  info.height = 10;
368  info.resolution = 1.0;
369 
370  nav_2d_msgs::Polygon2D simple_square;
371  simple_square.points.push_back(make_point(1.4, 1.4));
372  simple_square.points.push_back(make_point(1.4, 3.6));
373  simple_square.points.push_back(make_point(3.6, 3.6));
374  simple_square.points.push_back(make_point(3.6, 1.4));
375 
376  nav_grid_iterators::WholeGrid whole_grid(info);
377  whole_grid = whole_grid.begin();
378  nav_grid_iterators::SubGrid sub_grid(&info, 1, 2, 2, 3);
379  sub_grid = sub_grid.begin();
380  nav_grid_iterators::CircleFill cf(&info, 4.0, 4.0, 3.0);
381  cf = cf.begin();
382  nav_grid_iterators::CircleOutline co(&info, 4.0, 4.0, 3.0);
383  co = co.begin();
384  nav_grid_iterators::Spiral spiral(&info, 4.0, 4.0, 3.0);
385  spiral = spiral.begin();
386  nav_grid_iterators::Line line(&info, 0, 0, 0, 0);
387  line = line.begin();
388  nav_grid_iterators::PolygonOutline po(&info, simple_square);
389  po = po.begin();
390  nav_grid_iterators::PolygonFill pf(&info, simple_square);
391  pf = pf.begin();
392 }
393 
394 TEST(Iterators, test_assignment)
395 {
397  info.width = 10;
398  info.height = 10;
399  info.resolution = 1.0;
400  nav_grid_iterators::CircleFill iter1(&info, 3.0, 3.0, 1.0);
401  // Sequence should be (2, 2) (3, 2) (2, 3) (3, 3)
402  EXPECT_EQ((*iter1).x, 2U);
403  EXPECT_EQ((*iter1).y, 2U);
404 
405  nav_grid_iterators::CircleFill iter2 = iter1;
406 
407  // Effective Copy
408  EXPECT_EQ((*iter1).x, 2U);
409  EXPECT_EQ((*iter1).y, 2U);
410  EXPECT_EQ((*iter2).x, 2U);
411  EXPECT_EQ((*iter2).y, 2U);
412 
413  // Increment only iter2
414  ++iter2;
415  EXPECT_EQ((*iter1).x, 2U);
416  EXPECT_EQ((*iter1).y, 2U);
417  EXPECT_EQ((*iter2).x, 3U);
418  EXPECT_EQ((*iter2).y, 2U);
419 
420  // Increment first to match
421  ++iter1;
422  EXPECT_EQ((*iter1).x, 3U);
423  EXPECT_EQ((*iter1).y, 2U);
424  EXPECT_EQ((*iter2).x, 3U);
425  EXPECT_EQ((*iter2).y, 2U);
426 
427  // Increment only iter2
428  ++iter2;
429  EXPECT_EQ((*iter1).x, 3U);
430  EXPECT_EQ((*iter1).y, 2U);
431  EXPECT_EQ((*iter2).x, 2U);
432  EXPECT_EQ((*iter2).y, 3U);
433 
434  // Check copy when not at the start
435  nav_grid_iterators::CircleFill iter3 = iter1;
436  EXPECT_EQ((*iter1).x, 3U);
437  EXPECT_EQ((*iter1).y, 2U);
438  EXPECT_EQ((*iter2).x, 2U);
439  EXPECT_EQ((*iter2).y, 3U);
440  EXPECT_EQ((*iter3).x, 3U);
441  EXPECT_EQ((*iter3).y, 2U);
442 }
443 
444 int main(int argc, char **argv)
445 {
446  testing::InitGoogleTest(&argc, argv);
447  return RUN_ALL_TESTS();
448 }
Line begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Definition: line.cpp:136
int countIterations(iterator_type it, int max_iterations=1000)
Definition: utest.cpp:42
WholeGrid begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Definition: whole_grid.cpp:40
Iterates over all of the valid indexes on the outline of a polygon.
Iterates over all of the valid indexes that lie within a circle from the center out.
Definition: spiral.h:48
Iterates over all of the valid indexes that lie within a circle in row major order.
Definition: circle_fill.h:48
SubGrid begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Definition: sub_grid.cpp:39
GenericIndex< unsigned int > Index
int main(int argc, char **argv)
Definition: utest.cpp:444
nav_2d_msgs::Point2D make_point(double x, double y)
Definition: utest.cpp:288
Iterates over the valid indexes that lie on the outline of a circle.
WholeGrid end() const override
Helper function for range-style iteration.
Definition: whole_grid.cpp:45
CircleOutline begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Iterates over all of the valid indexes of a line.
Definition: line.h:48
Iterates over all of the valid indexes that lie within an arbitrary polygon in row major order...
Definition: polygon_fill.h:48
TEST(WholeGrid, whole_grid)
Definition: utest.cpp:54
PolygonFill begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Iterator for looping through every index within an aligned rectangular portion of the grid...
Definition: sub_grid.h:47
CircleFill begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Spiral begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Definition: spiral.cpp:89
PolygonOutline begin() const override
Helper function for range-style iteration Equivalent to the above constructor.


nav_grid_iterators
Author(s):
autogenerated on Wed Jun 26 2019 20:06:20