00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <gtest/gtest.h>
00035 #include <nav_grid/vector_nav_grid.h>
00036 #include <nav_grid/coordinate_conversion.h>
00037 #include <algorithm>
00038
00039 TEST(VectorNavGrid, info_equality)
00040 {
00041 nav_grid::NavGridInfo info0;
00042 nav_grid::NavGridInfo info1;
00043 nav_grid::NavGridInfo width_info;
00044 width_info.width = 3;
00045
00046 nav_grid::NavGridInfo height_info;
00047 height_info.height = 3;
00048
00049 nav_grid::NavGridInfo res_info;
00050 res_info.resolution = 3.0;
00051
00052 nav_grid::NavGridInfo frame_info;
00053 frame_info.frame_id = "foobar";
00054
00055 nav_grid::NavGridInfo originx_info;
00056 originx_info.origin_x = 3.0;
00057
00058 nav_grid::NavGridInfo originy_info;
00059 originy_info.origin_y = 3.0;
00060
00061
00062 EXPECT_EQ(info0, info0);
00063 EXPECT_EQ(info0, info1);
00064 EXPECT_NE(info0, width_info);
00065 EXPECT_NE(info0, height_info);
00066 EXPECT_NE(info0, res_info);
00067 EXPECT_NE(info0, frame_info);
00068 EXPECT_NE(info0, originx_info);
00069 EXPECT_NE(info0, originy_info);
00070 }
00071
00072 TEST(VectorNavGrid, basic_test)
00073 {
00074 nav_grid::VectorNavGrid<int> grid(-3);
00075 nav_grid::NavGridInfo info;
00076 info.width = 2;
00077 info.height = 3;
00078 grid.setInfo(info);
00079 EXPECT_EQ(grid(0, 0), -3);
00080 grid.setValue(1, 1, 10);
00081 EXPECT_EQ(grid(0, 0), -3);
00082 EXPECT_EQ(grid(1, 1), 10);
00083 }
00084
00085 TEST(VectorNavGrid, basic_index_test)
00086 {
00087 nav_grid::VectorNavGrid<int> grid(-3);
00088 nav_grid::NavGridInfo info;
00089 info.width = 2;
00090 info.height = 3;
00091 grid.setInfo(info);
00092
00093 nav_grid::Index index0(0, 0), index1(1, 1);
00094 EXPECT_EQ(grid(index0), -3);
00095 grid.setValue(index1, 10);
00096 EXPECT_EQ(grid(index0), -3);
00097 EXPECT_EQ(grid(index1), 10);
00098 EXPECT_EQ(grid(0, 0), -3);
00099 EXPECT_EQ(grid(1, 1), 10);
00100 }
00101
00102 TEST(VectorNavGrid, easy_coordinates_test)
00103 {
00104 nav_grid::NavGridInfo info;
00105 info.width = 2;
00106 info.height = 3;
00107
00108 double wx, wy;
00109 gridToWorld(info, 0, 0, wx, wy);
00110 EXPECT_DOUBLE_EQ(wx, 0.5);
00111 EXPECT_DOUBLE_EQ(wy, 0.5);
00112 gridToWorld(info, 1, 2, wx, wy);
00113 EXPECT_DOUBLE_EQ(wx, 1.5);
00114 EXPECT_DOUBLE_EQ(wy, 2.5);
00115
00116 unsigned int umx, umy;
00117 int mx, my;
00118 double dmx, dmy;
00119 ASSERT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
00120 EXPECT_EQ(umx, 1);
00121 EXPECT_EQ(umy, 2);
00122 worldToGrid(info, wx, wy, mx, my);
00123 EXPECT_EQ(mx, 1);
00124 EXPECT_EQ(my, 2);
00125 worldToGrid(info, wx, wy, dmx, dmy);
00126 EXPECT_DOUBLE_EQ(dmx, wx);
00127 EXPECT_DOUBLE_EQ(dmy, wy);
00128
00129
00130 wx = 2.5;
00131 EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
00132 EXPECT_EQ(umx, 1);
00133 EXPECT_EQ(umy, 2);
00134 worldToGrid(info, wx, wy, mx, my);
00135 EXPECT_EQ(mx, 2);
00136 EXPECT_EQ(my, 2);
00137
00138
00139 EXPECT_TRUE(worldToGridBounded(info, 0.0, wy, umx, umy));
00140 EXPECT_EQ(umx, 0);
00141 EXPECT_TRUE(worldToGridBounded(info, 0.25, wy, umx, umy));
00142 EXPECT_EQ(umx, 0);
00143 EXPECT_TRUE(worldToGridBounded(info, 0.75, wy, umx, umy));
00144 EXPECT_EQ(umx, 0);
00145 EXPECT_TRUE(worldToGridBounded(info, 0.9999, wy, umx, umy));
00146 EXPECT_EQ(umx, 0);
00147 EXPECT_TRUE(worldToGridBounded(info, 1.0, wy, umx, umy));
00148 EXPECT_EQ(umx, 1);
00149 EXPECT_TRUE(worldToGridBounded(info, 1.25, wy, umx, umy));
00150 EXPECT_EQ(umx, 1);
00151 EXPECT_TRUE(worldToGridBounded(info, 1.75, wy, umx, umy));
00152 EXPECT_EQ(umx, 1);
00153 EXPECT_TRUE(worldToGridBounded(info, 1.9999, wy, umx, umy));
00154 EXPECT_EQ(umx, 1);
00155 EXPECT_FALSE(worldToGridBounded(info, 2.0, wy, umx, umy));
00156 EXPECT_EQ(umx, 1);
00157 }
00158
00159 TEST(VectorNavGrid, hard_coordinates_test)
00160 {
00161 nav_grid::NavGridInfo info;
00162 info.width = 2;
00163 info.height = 3;
00164 info.resolution = 0.1;
00165 info.origin_x = -0.2;
00166 info.origin_y = 0.2;
00167
00168 double wx, wy;
00169 gridToWorld(info, 0, 0, wx, wy);
00170 EXPECT_DOUBLE_EQ(wx, -0.15);
00171 EXPECT_DOUBLE_EQ(wy, 0.25);
00172 gridToWorld(info, 1, 2, wx, wy);
00173 EXPECT_DOUBLE_EQ(wx, -0.05);
00174 EXPECT_DOUBLE_EQ(wy, 0.45);
00175
00176 unsigned int umx, umy;
00177 int mx, my;
00178 double dmx, dmy;
00179 EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
00180 EXPECT_EQ(umx, 1);
00181 EXPECT_EQ(umy, 2);
00182 worldToGrid(info, wx, wy, mx, my);
00183 EXPECT_EQ(mx, 1);
00184 EXPECT_EQ(my, 2);
00185 worldToGrid(info, wx, wy, dmx, dmy);
00186 EXPECT_DOUBLE_EQ(dmx, 1.5);
00187 EXPECT_DOUBLE_EQ(dmy, 2.5);
00188
00189
00190 wx = 2.5;
00191 EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
00192 EXPECT_EQ(umx, 1);
00193 EXPECT_EQ(umy, 2);
00194 worldToGrid(info, wx, wy, mx, my);
00195 EXPECT_EQ(mx, 27);
00196 EXPECT_EQ(my, 2);
00197 }
00198
00199
00200 TEST(VectorNavGrid, speed_test)
00201 {
00202 nav_grid::NavGridInfo info;
00203
00204 const int N = 1000;
00205 const int EXTRA = 300;
00206
00207 info.width = N;
00208 info.height = N;
00209
00210 double wx, wy;
00211 unsigned int umx, umy;
00212 int mx, my;
00213 double dmx, dmy;
00214
00215 for (int x = -EXTRA; x < N + EXTRA; x++)
00216 {
00217 for (int y = -EXTRA; y < N + EXTRA; y++)
00218 {
00219 gridToWorld(info, x, y, wx, wy);
00220 if (x < 0 || y < 0 || x >= N || y >= N)
00221 {
00222 EXPECT_FALSE(isWithinGrid(info, wx, wy));
00223 EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
00224 EXPECT_EQ(umx, std::min(std::max(0, x), N - 1));
00225 EXPECT_EQ(umy, std::min(std::max(0, y), N - 1));
00226 }
00227 else
00228 {
00229 EXPECT_TRUE(isWithinGrid(info, wx, wy));
00230 EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
00231 EXPECT_EQ(umx, x);
00232 EXPECT_EQ(umy, y);
00233 }
00234 worldToGrid(info, wx, wy, mx, my);
00235 EXPECT_EQ(mx, x);
00236 EXPECT_EQ(my, y);
00237 worldToGrid(info, wx, wy, dmx, dmy);
00238 EXPECT_DOUBLE_EQ(dmx, x + 0.5);
00239 EXPECT_DOUBLE_EQ(dmy, y + 0.5);
00240 }
00241 }
00242 }
00243
00244 int testGridValue(double x, double y)
00245 {
00246 return static_cast<int>(100 * floor(x) + floor(y));
00247 }
00248
00261 void initializeTestGrid(nav_grid::VectorNavGrid<int>& grid)
00262 {
00263 grid.setDefaultValue(-10);
00264 nav_grid::NavGridInfo info;
00265 info.width = 10;
00266 info.height = 5;
00267 grid.setInfo(info);
00268 double mx, my;
00269 for (unsigned int j = 0; j < info.height; j++)
00270 {
00271 for (unsigned int i = 0; i < info.width; i++)
00272 {
00273 gridToWorld(info, i, j, mx, my);
00274 grid.setValue(i, j, testGridValue(mx, my));
00275 }
00276 }
00277 }
00278
00282 void checkSetGridValues(const nav_grid::VectorNavGrid<int>& grid,
00283 unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
00284 {
00285 for (unsigned int x = 0; x < grid.getWidth(); x++)
00286 {
00287 for (unsigned int y = 0; y < grid.getHeight(); y++)
00288 {
00289 if (x >= x0 && x < x1 && y >= y0 && y < y1)
00290 {
00291 EXPECT_EQ(grid(x, y), testGridValue(x, y));
00292 }
00293 else
00294 {
00295 EXPECT_EQ(grid(x, y), -10);
00296 }
00297 }
00298 }
00299 }
00300
00304 void checkUpdateGridValues(const nav_grid::VectorNavGrid<int>& grid,
00305 unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
00306 {
00307 double mx, my;
00308 for (unsigned int x = 0; x < grid.getWidth(); x++)
00309 {
00310 for (unsigned int y = 0; y < grid.getHeight(); y++)
00311 {
00312 if (x >= x0 && x < x1 && y >= y0 && y < y1)
00313 {
00314 gridToWorld(grid.getInfo(), x, y, mx, my);
00315 EXPECT_EQ(grid(x, y), testGridValue(mx, my));
00316 }
00317 else
00318 {
00319 EXPECT_EQ(grid(x, y), -10);
00320 }
00321 }
00322 }
00323 }
00324
00325 void debugGridValues(const nav_grid::VectorNavGrid<int>& grid)
00326 {
00327 for (unsigned int j = 0; j < grid.getHeight(); j++)
00328 {
00329 for (unsigned int i = 0; i < grid.getWidth(); i++)
00330 {
00331 printf("%d ", grid(i, j));
00332 }
00333 printf("\n");
00334 }
00335 printf("\n");
00336 }
00337
00338 TEST(VectorNavGrid, resizing_grid_with_set)
00339 {
00340 nav_grid::VectorNavGrid<int> grid;
00341 initializeTestGrid(grid);
00342 checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
00343
00344 nav_grid::NavGridInfo decreased_width_info = grid.getInfo();
00345 decreased_width_info.width = 5;
00346 grid.setInfo(decreased_width_info);
00347 checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
00348
00349 nav_grid::NavGridInfo increased_width_info = grid.getInfo();
00350 increased_width_info.width = 9;
00351 grid.setInfo(increased_width_info);
00352 checkSetGridValues(grid, 0, 5, 0, grid.getHeight());
00353
00354 initializeTestGrid(grid);
00355 checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
00356
00357 nav_grid::NavGridInfo increased_height_info = grid.getInfo();
00358 increased_height_info.height = 9;
00359 grid.setInfo(increased_height_info);
00360 checkSetGridValues(grid, 0, grid.getWidth(), 0, 5);
00361
00362 nav_grid::NavGridInfo decreased_height_info = grid.getInfo();
00363 decreased_height_info.height = 4;
00364 grid.setInfo(decreased_height_info);
00365 checkSetGridValues(grid, 0, grid.getWidth(), 0, 4);
00366 }
00367
00368 TEST(VectorNavGrid, resizing_grid_with_update)
00369 {
00370 nav_grid::VectorNavGrid<int> grid;
00371 initializeTestGrid(grid);
00372 checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
00373
00374 nav_grid::NavGridInfo decreased_width_info = grid.getInfo();
00375 decreased_width_info.width = 5;
00376 grid.updateInfo(decreased_width_info);
00377 checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
00378
00379 nav_grid::NavGridInfo increased_width_info = grid.getInfo();
00380 increased_width_info.width = 9;
00381 grid.updateInfo(increased_width_info);
00382 checkUpdateGridValues(grid, 0, 5, 0, grid.getHeight());
00383
00384 initializeTestGrid(grid);
00385 checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
00386
00387 nav_grid::NavGridInfo increased_height_info = grid.getInfo();
00388 increased_height_info.height = 9;
00389 grid.updateInfo(increased_height_info);
00390 checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 5);
00391
00392 nav_grid::NavGridInfo decreased_height_info = grid.getInfo();
00393 decreased_height_info.height = 4;
00394 grid.updateInfo(decreased_height_info);
00395 checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 4);
00396 }
00397
00398 TEST(VectorNavGrid, change_origin)
00399 {
00400 nav_grid::VectorNavGrid<int> grid;
00401 initializeTestGrid(grid);
00402
00403 nav_grid::NavGridInfo bump_right_info = grid.getInfo();
00404 bump_right_info.origin_x = 3;
00405 grid.updateInfo(bump_right_info);
00406 checkUpdateGridValues(grid, 0, 7, 0, grid.getHeight());
00407
00408 nav_grid::NavGridInfo bump_up_info = grid.getInfo();
00409 bump_up_info.origin_y = 2;
00410 grid.updateInfo(bump_up_info);
00411 checkUpdateGridValues(grid, 0, 7, 0, 3);
00412
00413 nav_grid::NavGridInfo bump_left_info = grid.getInfo();
00414 bump_left_info.origin_x = -1;
00415 grid.updateInfo(bump_left_info);
00416 checkUpdateGridValues(grid, 4, grid.getWidth(), 0, 3);
00417
00418 nav_grid::NavGridInfo bump_down_info = grid.getInfo();
00419 bump_down_info.origin_y = 0;
00420 grid.updateInfo(bump_down_info);
00421 checkUpdateGridValues(grid, 4, grid.getWidth(), 2, grid.getHeight());
00422
00423
00424 initializeTestGrid(grid);
00425 nav_grid::NavGridInfo bump_far_right_info = grid.getInfo();
00426 bump_far_right_info.origin_x = 30;
00427 grid.updateInfo(bump_far_right_info);
00428 checkUpdateGridValues(grid, 0, 0, 0, 0);
00429 }
00430
00431 TEST(VectorNavGrid, combined_changes)
00432 {
00433
00434 nav_grid::VectorNavGrid<int> grid;
00435 initializeTestGrid(grid);
00436 checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
00437
00438 nav_grid::NavGridInfo info1 = grid.getInfo();
00439 info1.width = 15;
00440 info1.origin_x = -5.0;
00441 grid.updateInfo(info1);
00442 checkUpdateGridValues(grid, 5, grid.getWidth(), 0, grid.getHeight());
00443
00444 initializeTestGrid(grid);
00445 nav_grid::NavGridInfo info2 = grid.getInfo();
00446 info2.width = 17;
00447 info2.origin_x = -5.0;
00448 grid.updateInfo(info2);
00449 checkUpdateGridValues(grid, 5, grid.getWidth() - 2, 0, grid.getHeight());
00450
00451 initializeTestGrid(grid);
00452 nav_grid::NavGridInfo info3 = grid.getInfo();
00453 info3.width = 2;
00454 info3.origin_x = 2.0;
00455 grid.updateInfo(info3);
00456 checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
00457
00458 initializeTestGrid(grid);
00459 nav_grid::NavGridInfo info4 = grid.getInfo();
00460 info4.width = 20;
00461 info4.height = 20;
00462 info4.origin_x = -2.0;
00463 info4.origin_y = -5.0;
00464 grid.updateInfo(info4);
00465 checkUpdateGridValues(grid, 2, 12, 5, 10);
00466 }
00467
00468 TEST(Index, comparison_tests)
00469 {
00470 unsigned int N = 5;
00471 for (unsigned int x0 = 0; x0 < N; ++x0)
00472 {
00473 for (unsigned int y0 = 0; y0 < N; ++y0)
00474 {
00475 nav_grid::Index index0(x0, y0);
00476
00477 for (unsigned int x1 = 0; x1 < N; ++x1)
00478 {
00479 for (unsigned int y1 = 0; y1 < N; ++y1)
00480 {
00481 nav_grid::Index index1(x1, y1);
00482
00483
00484 if (x0 == x1 && y0 == y1)
00485 {
00486 EXPECT_EQ(index0, index1);
00487 EXPECT_TRUE(!(index0 < index1) && !(index1 < index0));
00488 EXPECT_GE(index0, index1);
00489 EXPECT_LE(index0, index1);
00490 EXPECT_GE(index1, index0);
00491 EXPECT_LE(index1, index0);
00492 }
00493 else
00494 {
00495 EXPECT_NE(index0, index1);
00496 EXPECT_FALSE(!(index0 < index1) && !(index1 < index0));
00497 if (x0 < x1 || (x0 == x1 && y0 < y1))
00498 {
00499 EXPECT_LT(index0, index1);
00500 EXPECT_GT(index1, index0);
00501 EXPECT_LE(index0, index1);
00502 EXPECT_GE(index1, index0);
00503 }
00504 else
00505 {
00506 EXPECT_GT(index0, index1);
00507 EXPECT_LT(index1, index0);
00508 EXPECT_GE(index0, index1);
00509 EXPECT_LE(index1, index0);
00510 }
00511 }
00512 }
00513 }
00514 }
00515 }
00516 }
00517
00518 int main(int argc, char **argv)
00519 {
00520 testing::InitGoogleTest(&argc, argv);
00521 return RUN_ALL_TESTS();
00522 }