00001
00002
00003
00004
00005
00006
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
00014 #include <Eigen/Core>
00015
00016
00017 #include <gtest/gtest.h>
00018
00019
00020 #include <cfloat>
00021
00022
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));
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));
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));
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));
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
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));
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 }