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>
37 #include <algorithm>
38 
39 TEST(VectorNavGrid, info_equality)
40 {
43  nav_grid::NavGridInfo width_info;
44  width_info.width = 3;
45 
46  nav_grid::NavGridInfo height_info;
47  height_info.height = 3;
48 
49  nav_grid::NavGridInfo res_info;
50  res_info.resolution = 3.0;
51 
52  nav_grid::NavGridInfo frame_info;
53  frame_info.frame_id = "foobar";
54 
55  nav_grid::NavGridInfo originx_info;
56  originx_info.origin_x = 3.0;
57 
58  nav_grid::NavGridInfo originy_info;
59  originy_info.origin_y = 3.0;
60 
61 
62  EXPECT_EQ(info0, info0);
63  EXPECT_EQ(info0, info1);
64  EXPECT_NE(info0, width_info);
65  EXPECT_NE(info0, height_info);
66  EXPECT_NE(info0, res_info);
67  EXPECT_NE(info0, frame_info);
68  EXPECT_NE(info0, originx_info);
69  EXPECT_NE(info0, originy_info);
70 }
71 
72 TEST(VectorNavGrid, basic_test)
73 {
76  info.width = 2;
77  info.height = 3;
78  grid.setInfo(info);
79  EXPECT_EQ(grid(0, 0), -3);
80  grid.setValue(1, 1, 10);
81  EXPECT_EQ(grid(0, 0), -3);
82  EXPECT_EQ(grid(1, 1), 10);
83 }
84 
85 TEST(VectorNavGrid, basic_index_test)
86 {
89  info.width = 2;
90  info.height = 3;
91  grid.setInfo(info);
92 
93  nav_grid::Index index0(0, 0), index1(1, 1);
94  EXPECT_EQ(grid(index0), -3);
95  grid.setValue(index1, 10);
96  EXPECT_EQ(grid(index0), -3);
97  EXPECT_EQ(grid(index1), 10);
98  EXPECT_EQ(grid(0, 0), -3);
99  EXPECT_EQ(grid(1, 1), 10);
100 }
101 
102 TEST(VectorNavGrid, easy_coordinates_test)
103 {
105  info.width = 2;
106  info.height = 3;
107 
108  double wx, wy;
109  gridToWorld(info, 0, 0, wx, wy);
110  EXPECT_DOUBLE_EQ(wx, 0.5);
111  EXPECT_DOUBLE_EQ(wy, 0.5);
112  gridToWorld(info, 1, 2, wx, wy);
113  EXPECT_DOUBLE_EQ(wx, 1.5);
114  EXPECT_DOUBLE_EQ(wy, 2.5);
115 
116  unsigned int umx, umy;
117  int mx, my;
118  double dmx, dmy;
119  ASSERT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
120  EXPECT_EQ(umx, 1);
121  EXPECT_EQ(umy, 2);
122  worldToGrid(info, wx, wy, mx, my);
123  EXPECT_EQ(mx, 1);
124  EXPECT_EQ(my, 2);
125  worldToGrid(info, wx, wy, dmx, dmy);
126  EXPECT_DOUBLE_EQ(dmx, wx);
127  EXPECT_DOUBLE_EQ(dmy, wy);
128 
129  // Invalid Coordinate
130  wx = 2.5;
131  EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
132  EXPECT_EQ(umx, 1);
133  EXPECT_EQ(umy, 2);
134  worldToGrid(info, wx, wy, mx, my);
135  EXPECT_EQ(mx, 2);
136  EXPECT_EQ(my, 2);
137 
138  // Border Cases
139  EXPECT_TRUE(worldToGridBounded(info, 0.0, wy, umx, umy));
140  EXPECT_EQ(umx, 0);
141  EXPECT_TRUE(worldToGridBounded(info, 0.25, wy, umx, umy));
142  EXPECT_EQ(umx, 0);
143  EXPECT_TRUE(worldToGridBounded(info, 0.75, wy, umx, umy));
144  EXPECT_EQ(umx, 0);
145  EXPECT_TRUE(worldToGridBounded(info, 0.9999, wy, umx, umy));
146  EXPECT_EQ(umx, 0);
147  EXPECT_TRUE(worldToGridBounded(info, 1.0, wy, umx, umy));
148  EXPECT_EQ(umx, 1);
149  EXPECT_TRUE(worldToGridBounded(info, 1.25, wy, umx, umy));
150  EXPECT_EQ(umx, 1);
151  EXPECT_TRUE(worldToGridBounded(info, 1.75, wy, umx, umy));
152  EXPECT_EQ(umx, 1);
153  EXPECT_TRUE(worldToGridBounded(info, 1.9999, wy, umx, umy));
154  EXPECT_EQ(umx, 1);
155  EXPECT_FALSE(worldToGridBounded(info, 2.0, wy, umx, umy));
156  EXPECT_EQ(umx, 1);
157 }
158 
159 TEST(VectorNavGrid, hard_coordinates_test)
160 {
162  info.width = 2;
163  info.height = 3;
164  info.resolution = 0.1;
165  info.origin_x = -0.2;
166  info.origin_y = 0.2;
167 
168  double wx, wy;
169  gridToWorld(info, 0, 0, wx, wy);
170  EXPECT_DOUBLE_EQ(wx, -0.15);
171  EXPECT_DOUBLE_EQ(wy, 0.25);
172  gridToWorld(info, 1, 2, wx, wy);
173  EXPECT_DOUBLE_EQ(wx, -0.05);
174  EXPECT_DOUBLE_EQ(wy, 0.45);
175 
176  unsigned int umx, umy;
177  int mx, my;
178  double dmx, dmy;
179  EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
180  EXPECT_EQ(umx, 1);
181  EXPECT_EQ(umy, 2);
182  worldToGrid(info, wx, wy, mx, my);
183  EXPECT_EQ(mx, 1);
184  EXPECT_EQ(my, 2);
185  worldToGrid(info, wx, wy, dmx, dmy);
186  EXPECT_DOUBLE_EQ(dmx, 1.5);
187  EXPECT_DOUBLE_EQ(dmy, 2.5);
188 
189  // Invalid Coordinate
190  wx = 2.5;
191  EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
192  EXPECT_EQ(umx, 1);
193  EXPECT_EQ(umy, 2);
194  worldToGrid(info, wx, wy, mx, my);
195  EXPECT_EQ(mx, 27);
196  EXPECT_EQ(my, 2);
197 }
198 
199 
200 TEST(VectorNavGrid, speed_test)
201 {
203 
204  const int N = 1000;
205  const int EXTRA = 300;
206 
207  info.width = N;
208  info.height = N;
209 
210  double wx, wy;
211  unsigned int umx, umy;
212  int mx, my;
213  double dmx, dmy;
214 
215  for (int x = -EXTRA; x < N + EXTRA; x++)
216  {
217  for (int y = -EXTRA; y < N + EXTRA; y++)
218  {
219  gridToWorld(info, x, y, wx, wy);
220  if (x < 0 || y < 0 || x >= N || y >= N)
221  {
222  EXPECT_FALSE(isWithinGrid(info, wx, wy));
223  EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
224  EXPECT_EQ(umx, std::min(std::max(0, x), N - 1));
225  EXPECT_EQ(umy, std::min(std::max(0, y), N - 1));
226  }
227  else
228  {
229  EXPECT_TRUE(isWithinGrid(info, wx, wy));
230  EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
231  EXPECT_EQ(umx, x);
232  EXPECT_EQ(umy, y);
233  }
234  worldToGrid(info, wx, wy, mx, my);
235  EXPECT_EQ(mx, x);
236  EXPECT_EQ(my, y);
237  worldToGrid(info, wx, wy, dmx, dmy);
238  EXPECT_DOUBLE_EQ(dmx, x + 0.5);
239  EXPECT_DOUBLE_EQ(dmy, y + 0.5);
240  }
241  }
242 }
243 
244 int testGridValue(double x, double y)
245 {
246  return static_cast<int>(100 * floor(x) + floor(y));
247 }
248 
262 {
263  grid.setDefaultValue(-10);
265  info.width = 10;
266  info.height = 5;
267  grid.setInfo(info);
268  double mx, my;
269  for (unsigned int j = 0; j < info.height; j++)
270  {
271  for (unsigned int i = 0; i < info.width; i++)
272  {
273  gridToWorld(info, i, j, mx, my);
274  grid.setValue(i, j, testGridValue(mx, my));
275  }
276  }
277 }
278 
283  unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
284 {
285  for (unsigned int x = 0; x < grid.getWidth(); x++)
286  {
287  for (unsigned int y = 0; y < grid.getHeight(); y++)
288  {
289  if (x >= x0 && x < x1 && y >= y0 && y < y1)
290  {
291  EXPECT_EQ(grid(x, y), testGridValue(x, y)); // testGridValue based on Grid Coordinates
292  }
293  else
294  {
295  EXPECT_EQ(grid(x, y), -10);
296  }
297  }
298  }
299 }
300 
305  unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
306 {
307  double mx, my;
308  for (unsigned int x = 0; x < grid.getWidth(); x++)
309  {
310  for (unsigned int y = 0; y < grid.getHeight(); y++)
311  {
312  if (x >= x0 && x < x1 && y >= y0 && y < y1)
313  {
314  gridToWorld(grid.getInfo(), x, y, mx, my);
315  EXPECT_EQ(grid(x, y), testGridValue(mx, my)); // testGridValue based on World Coordinates
316  }
317  else
318  {
319  EXPECT_EQ(grid(x, y), -10);
320  }
321  }
322  }
323 }
324 
326 {
327  for (unsigned int j = 0; j < grid.getHeight(); j++)
328  {
329  for (unsigned int i = 0; i < grid.getWidth(); i++)
330  {
331  printf("%d ", grid(i, j));
332  }
333  printf("\n");
334  }
335  printf("\n");
336 }
337 
338 TEST(VectorNavGrid, resizing_grid_with_set)
339 {
341  initializeTestGrid(grid);
342  checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
343 
344  nav_grid::NavGridInfo decreased_width_info = grid.getInfo();
345  decreased_width_info.width = 5;
346  grid.setInfo(decreased_width_info);
347  checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
348 
349  nav_grid::NavGridInfo increased_width_info = grid.getInfo();
350  increased_width_info.width = 9;
351  grid.setInfo(increased_width_info);
352  checkSetGridValues(grid, 0, 5, 0, grid.getHeight());
353 
354  initializeTestGrid(grid);
355  checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
356 
357  nav_grid::NavGridInfo increased_height_info = grid.getInfo();
358  increased_height_info.height = 9;
359  grid.setInfo(increased_height_info);
360  checkSetGridValues(grid, 0, grid.getWidth(), 0, 5);
361 
362  nav_grid::NavGridInfo decreased_height_info = grid.getInfo();
363  decreased_height_info.height = 4;
364  grid.setInfo(decreased_height_info);
365  checkSetGridValues(grid, 0, grid.getWidth(), 0, 4);
366 }
367 
368 TEST(VectorNavGrid, resizing_grid_with_update)
369 {
371  initializeTestGrid(grid);
372  checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
373 
374  nav_grid::NavGridInfo decreased_width_info = grid.getInfo();
375  decreased_width_info.width = 5;
376  grid.updateInfo(decreased_width_info);
377  checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
378 
379  nav_grid::NavGridInfo increased_width_info = grid.getInfo();
380  increased_width_info.width = 9;
381  grid.updateInfo(increased_width_info);
382  checkUpdateGridValues(grid, 0, 5, 0, grid.getHeight());
383 
384  initializeTestGrid(grid);
385  checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
386 
387  nav_grid::NavGridInfo increased_height_info = grid.getInfo();
388  increased_height_info.height = 9;
389  grid.updateInfo(increased_height_info);
390  checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 5);
391 
392  nav_grid::NavGridInfo decreased_height_info = grid.getInfo();
393  decreased_height_info.height = 4;
394  grid.updateInfo(decreased_height_info);
395  checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 4);
396 }
397 
398 TEST(VectorNavGrid, change_origin)
399 {
401  initializeTestGrid(grid);
402 
403  nav_grid::NavGridInfo bump_right_info = grid.getInfo();
404  bump_right_info.origin_x = 3;
405  grid.updateInfo(bump_right_info);
406  checkUpdateGridValues(grid, 0, 7, 0, grid.getHeight());
407 
408  nav_grid::NavGridInfo bump_up_info = grid.getInfo();
409  bump_up_info.origin_y = 2;
410  grid.updateInfo(bump_up_info);
411  checkUpdateGridValues(grid, 0, 7, 0, 3);
412 
413  nav_grid::NavGridInfo bump_left_info = grid.getInfo();
414  bump_left_info.origin_x = -1;
415  grid.updateInfo(bump_left_info);
416  checkUpdateGridValues(grid, 4, grid.getWidth(), 0, 3);
417 
418  nav_grid::NavGridInfo bump_down_info = grid.getInfo();
419  bump_down_info.origin_y = 0;
420  grid.updateInfo(bump_down_info);
421  checkUpdateGridValues(grid, 4, grid.getWidth(), 2, grid.getHeight());
422 
423 
424  initializeTestGrid(grid);
425  nav_grid::NavGridInfo bump_far_right_info = grid.getInfo();
426  bump_far_right_info.origin_x = 30;
427  grid.updateInfo(bump_far_right_info);
428  checkUpdateGridValues(grid, 0, 0, 0, 0);
429 }
430 
431 TEST(VectorNavGrid, combined_changes)
432 {
433  // This is not a complete set of possible combined changes, just enough to satisfy my curiousity
435  initializeTestGrid(grid);
436  checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
437 
438  nav_grid::NavGridInfo info1 = grid.getInfo();
439  info1.width = 15;
440  info1.origin_x = -5.0;
441  grid.updateInfo(info1);
442  checkUpdateGridValues(grid, 5, grid.getWidth(), 0, grid.getHeight());
443 
444  initializeTestGrid(grid);
445  nav_grid::NavGridInfo info2 = grid.getInfo();
446  info2.width = 17;
447  info2.origin_x = -5.0;
448  grid.updateInfo(info2);
449  checkUpdateGridValues(grid, 5, grid.getWidth() - 2, 0, grid.getHeight());
450 
451  initializeTestGrid(grid);
452  nav_grid::NavGridInfo info3 = grid.getInfo();
453  info3.width = 2;
454  info3.origin_x = 2.0;
455  grid.updateInfo(info3);
456  checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
457 
458  initializeTestGrid(grid);
459  nav_grid::NavGridInfo info4 = grid.getInfo();
460  info4.width = 20;
461  info4.height = 20;
462  info4.origin_x = -2.0;
463  info4.origin_y = -5.0;
464  grid.updateInfo(info4);
465  checkUpdateGridValues(grid, 2, 12, 5, 10);
466 }
467 
468 TEST(Index, comparison_tests)
469 {
470  unsigned int N = 5;
471  for (unsigned int x0 = 0; x0 < N; ++x0)
472  {
473  for (unsigned int y0 = 0; y0 < N; ++y0)
474  {
475  nav_grid::Index index0(x0, y0);
476 
477  for (unsigned int x1 = 0; x1 < N; ++x1)
478  {
479  for (unsigned int y1 = 0; y1 < N; ++y1)
480  {
481  nav_grid::Index index1(x1, y1);
482  // Check equality and the test for equality that sets use
483  // See https://stackoverflow.com/a/1114862
484  if (x0 == x1 && y0 == y1)
485  {
486  EXPECT_EQ(index0, index1);
487  EXPECT_TRUE(!(index0 < index1) && !(index1 < index0));
488  EXPECT_GE(index0, index1);
489  EXPECT_LE(index0, index1);
490  EXPECT_GE(index1, index0);
491  EXPECT_LE(index1, index0);
492  }
493  else
494  {
495  EXPECT_NE(index0, index1);
496  EXPECT_FALSE(!(index0 < index1) && !(index1 < index0));
497  if (x0 < x1 || (x0 == x1 && y0 < y1))
498  {
499  EXPECT_LT(index0, index1);
500  EXPECT_GT(index1, index0);
501  EXPECT_LE(index0, index1);
502  EXPECT_GE(index1, index0);
503  }
504  else
505  {
506  EXPECT_GT(index0, index1);
507  EXPECT_LT(index1, index0);
508  EXPECT_GE(index0, index1);
509  EXPECT_LE(index1, index0);
510  }
511  }
512  }
513  }
514  }
515  }
516 }
517 
518 int main(int argc, char **argv)
519 {
520  testing::InitGoogleTest(&argc, argv);
521  return RUN_ALL_TESTS();
522 }
void debugGridValues(const nav_grid::VectorNavGrid< int > &grid)
Definition: utest.cpp:325
double origin_x
The origin defines the coordinates of minimum corner of cell (0,0) in the grid.
Definition: nav_grid_info.h:57
void gridToWorld(const NavGridInfo &info, int mx, int my, double &wx, double &wy)
Convert from grid coordinates to world coordinates of the center of the cell.
void setDefaultValue(const T new_value)
Set the default value.
Definition: nav_grid.h:135
GenericIndex< unsigned int > Index
Definition: index.h:95
bool isWithinGrid(const NavGridInfo &info, double wx, double wy)
Check to see if the world coordinates are within the grid.
void checkSetGridValues(const nav_grid::VectorNavGrid< int > &grid, unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
Definition: utest.cpp:282
int main(int argc, char **argv)
Definition: utest.cpp:518
void initializeTestGrid(nav_grid::VectorNavGrid< int > &grid)
Definition: utest.cpp:261
void setInfo(const NavGridInfo &new_info) override
Change the info while attempting to keep the values associated with the grid coordinates.
unsigned int getHeight() const
Definition: nav_grid.h:144
void setValue(const unsigned int x, const unsigned int y, const T &value) override
set the value of the grid at (x,y)
void updateInfo(const NavGridInfo &new_info) override
Update the info while keeping the data geometrically in tact.
unsigned int getWidth() const
Definition: nav_grid.h:143
NavGridInfo getInfo() const
Definition: nav_grid.h:129
void worldToGrid(const NavGridInfo &info, double wx, double wy, double &mx, double &my)
Convert from world coordinates to the precise (double) grid coordinates.
A simple pair of x/y coordinates.
Definition: index.h:47
int testGridValue(double x, double y)
Definition: utest.cpp:244
TEST(VectorNavGrid, info_equality)
Definition: utest.cpp:39
void checkUpdateGridValues(const nav_grid::VectorNavGrid< int > &grid, unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
Definition: utest.cpp:304
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)
Convert from world coordinates to grid coordinates.


nav_grid
Author(s):
autogenerated on Sun Jan 10 2021 04:08:26