PolygonIteratorTest.cpp
Go to the documentation of this file.
00001 /*
00002  * PolygonIteratorTest.cpp
00003  *
00004  *  Created on: Sep 19, 2014
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_core/iterators/PolygonIterator.hpp"
00010 #include "grid_map_core/GridMap.hpp"
00011 #include "grid_map_core/Polygon.hpp"
00012 
00013 // Eigen
00014 #include <Eigen/Core>
00015 
00016 // gtest
00017 #include <gtest/gtest.h>
00018 
00019 // Limits
00020 #include <cfloat>
00021 
00022 // Vector
00023 #include <vector>
00024 
00025 using namespace std;
00026 using namespace Eigen;
00027 using namespace grid_map;
00028 
00029 TEST(PolygonIterator, FullCover)
00030 {
00031   vector<string> types;
00032   types.push_back("type");
00033   GridMap map(types);
00034   map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00035 
00036   Polygon polygon;
00037   polygon.addVertex(Position(-100.0, 100.0));
00038   polygon.addVertex(Position(100.0, 100.0));
00039   polygon.addVertex(Position(100.0, -100.0));
00040   polygon.addVertex(Position(-100.0, -100.0));
00041 
00042   PolygonIterator iterator(map, polygon);
00043 
00044   EXPECT_FALSE(iterator.isPastEnd());
00045   EXPECT_EQ(0, (*iterator)(0));
00046   EXPECT_EQ(0, (*iterator)(1));
00047 
00048   ++iterator;
00049   EXPECT_FALSE(iterator.isPastEnd());
00050   EXPECT_EQ(0, (*iterator)(0));
00051   EXPECT_EQ(1, (*iterator)(1));
00052 
00053   ++iterator;
00054   EXPECT_FALSE(iterator.isPastEnd());
00055   EXPECT_EQ(0, (*iterator)(0));
00056   EXPECT_EQ(2, (*iterator)(1));
00057 
00058   for (int i = 0; i < 37; ++i) ++iterator;
00059 
00060   EXPECT_FALSE(iterator.isPastEnd());
00061   EXPECT_EQ(7, (*iterator)(0));
00062   EXPECT_EQ(4, (*iterator)(1));
00063 
00064   ++iterator;
00065   EXPECT_TRUE(iterator.isPastEnd());
00066 }
00067 
00068 TEST(PolygonIterator, Outside)
00069 {
00070   GridMap map({"types"});
00071   map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00072 
00073   Polygon polygon;
00074   polygon.addVertex(Position(99.0, 101.0));
00075   polygon.addVertex(Position(101.0, 101.0));
00076   polygon.addVertex(Position(101.0, 99.0));
00077   polygon.addVertex(Position(99.0, 99.0));
00078 
00079   PolygonIterator iterator(map, polygon);
00080 
00081   EXPECT_TRUE(iterator.isPastEnd());
00082 }
00083 
00084 TEST(PolygonIterator, Square)
00085 {
00086   GridMap map({"types"});
00087   map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00088 
00089   Polygon polygon;
00090   polygon.addVertex(Position(-1.0, 1.5));
00091   polygon.addVertex(Position(1.0, 1.5));
00092   polygon.addVertex(Position(1.0, -1.5));
00093   polygon.addVertex(Position(-1.0, -1.5));
00094 
00095   PolygonIterator iterator(map, polygon);
00096 
00097   EXPECT_FALSE(iterator.isPastEnd());
00098   EXPECT_EQ(3, (*iterator)(0));
00099   EXPECT_EQ(1, (*iterator)(1));
00100 
00101   ++iterator;
00102   EXPECT_FALSE(iterator.isPastEnd());
00103   EXPECT_EQ(3, (*iterator)(0));
00104   EXPECT_EQ(2, (*iterator)(1));
00105 
00106   ++iterator;
00107   EXPECT_FALSE(iterator.isPastEnd());
00108   EXPECT_EQ(3, (*iterator)(0));
00109   EXPECT_EQ(3, (*iterator)(1));
00110 
00111   ++iterator;
00112   EXPECT_FALSE(iterator.isPastEnd());
00113   EXPECT_EQ(4, (*iterator)(0));
00114   EXPECT_EQ(1, (*iterator)(1));
00115 
00116   ++iterator;
00117   EXPECT_FALSE(iterator.isPastEnd());
00118   EXPECT_EQ(4, (*iterator)(0));
00119   EXPECT_EQ(2, (*iterator)(1));
00120 
00121   ++iterator;
00122   EXPECT_FALSE(iterator.isPastEnd());
00123   EXPECT_EQ(4, (*iterator)(0));
00124   EXPECT_EQ(3, (*iterator)(1));
00125 
00126   ++iterator;
00127   EXPECT_TRUE(iterator.isPastEnd());
00128 }
00129 
00130 TEST(PolygonIterator, TopLeftTriangle)
00131 {
00132   GridMap map({"types"});
00133   map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00134 
00135   Polygon polygon;
00136   polygon.addVertex(Position(-40.1, 20.6));
00137   polygon.addVertex(Position(40.1, 20.4));
00138   polygon.addVertex(Position(-40.1, -20.6));
00139 
00140   PolygonIterator iterator(map, polygon);
00141 
00142   EXPECT_FALSE(iterator.isPastEnd());
00143   EXPECT_EQ(0, (*iterator)(0));
00144   EXPECT_EQ(0, (*iterator)(1));
00145 
00146   ++iterator;
00147   EXPECT_FALSE(iterator.isPastEnd());
00148   EXPECT_EQ(1, (*iterator)(0));
00149   EXPECT_EQ(0, (*iterator)(1));
00150 
00151   // TODO Extend.
00152 }
00153 
00154 TEST(PolygonIterator, MoveMap)
00155 {
00156   GridMap map({"layer"});
00157   map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
00158   map.move(Position(2.0, 0.0));
00159 
00160   Polygon polygon;
00161   polygon.addVertex(Position(6.1, 1.6));
00162   polygon.addVertex(Position(0.9, 1.6));
00163   polygon.addVertex(Position(0.9, -1.6));
00164   polygon.addVertex(Position(6.1, -1.6));
00165   PolygonIterator iterator(map, polygon);
00166 
00167   EXPECT_FALSE(iterator.isPastEnd());
00168   EXPECT_EQ(6, (*iterator)(0));
00169   EXPECT_EQ(1, (*iterator)(1));
00170 
00171   ++iterator;
00172   EXPECT_FALSE(iterator.isPastEnd());
00173   EXPECT_EQ(6, (*iterator)(0));
00174   EXPECT_EQ(2, (*iterator)(1));
00175 
00176   for (int i = 0; i < 4; ++i) ++iterator;
00177 
00178   EXPECT_FALSE(iterator.isPastEnd());
00179   EXPECT_EQ(7, (*iterator)(0));
00180   EXPECT_EQ(3, (*iterator)(1));
00181 
00182   ++iterator;
00183   EXPECT_FALSE(iterator.isPastEnd());
00184   EXPECT_EQ(0, (*iterator)(0));
00185   EXPECT_EQ(1, (*iterator)(1));
00186 
00187   for (int i = 0; i < 8; ++i) ++iterator;
00188 
00189   EXPECT_FALSE(iterator.isPastEnd());
00190   EXPECT_EQ(2, (*iterator)(0));
00191   EXPECT_EQ(3, (*iterator)(1));
00192 
00193   ++iterator;
00194   EXPECT_TRUE(iterator.isPastEnd());
00195 }


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:13