utest.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2018, Locus Robotics
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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   // Invalid Coordinate
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   // Border Cases
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   // Invalid Coordinate
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));  // testGridValue based on Grid Coordinates
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));  // testGridValue based on World Coordinates
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   // This is not a complete set of possible combined changes, just enough to satisfy my curiousity
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           // Check equality and the test for equality that sets use
00483           // See https://stackoverflow.com/a/1114862
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 }


nav_grid
Author(s):
autogenerated on Wed Jun 26 2019 20:09:30