00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_core/iterators/LineIterator.hpp"
00010 #include "grid_map_core/GridMap.hpp"
00011
00012
00013 #include <gtest/gtest.h>
00014
00015
00016 #include <cfloat>
00017
00018 using namespace grid_map;
00019
00020 TEST(LineIterator, StartOutsideMap)
00021 {
00022 GridMap map( { "types" });
00023 map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
00024
00025 LineIterator iterator(map, Position(2.0, 2.0), Position(0.0, 0.0));
00026
00027 EXPECT_FALSE(iterator.isPastEnd());
00028 EXPECT_EQ(2, (*iterator)(0));
00029 EXPECT_EQ(0, (*iterator)(1));
00030
00031 ++iterator;
00032 EXPECT_FALSE(iterator.isPastEnd());
00033 EXPECT_EQ(3, (*iterator)(0));
00034 EXPECT_EQ(1, (*iterator)(1));
00035
00036 ++iterator;
00037 EXPECT_FALSE(iterator.isPastEnd());
00038 EXPECT_EQ(4, (*iterator)(0));
00039 EXPECT_EQ(2, (*iterator)(1));
00040
00041 ++iterator;
00042 EXPECT_TRUE(iterator.isPastEnd());
00043 }
00044
00045 TEST(LineIterator, EndOutsideMap)
00046 {
00047 GridMap map( { "types" });
00048 map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
00049
00050 LineIterator iterator(map, Position(0.0, 0.0), Position(9.0, 6.0));
00051
00052 EXPECT_FALSE(iterator.isPastEnd());
00053 EXPECT_EQ(4, (*iterator)(0));
00054 EXPECT_EQ(2, (*iterator)(1));
00055
00056 ++iterator;
00057 EXPECT_FALSE(iterator.isPastEnd());
00058 EXPECT_EQ(3, (*iterator)(0));
00059 EXPECT_EQ(1, (*iterator)(1));
00060
00061 ++iterator;
00062 EXPECT_FALSE(iterator.isPastEnd());
00063 EXPECT_EQ(2, (*iterator)(0));
00064 EXPECT_EQ(1, (*iterator)(1));
00065
00066 ++iterator;
00067 EXPECT_FALSE(iterator.isPastEnd());
00068 ++iterator;
00069 EXPECT_FALSE(iterator.isPastEnd());
00070 ++iterator;
00071 EXPECT_TRUE(iterator.isPastEnd());
00072 }
00073
00074 TEST(LineIterator, StartAndEndOutsideMap)
00075 {
00076 GridMap map( { "types" });
00077 map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
00078
00079 LineIterator iterator(map, Position(-7.0, -9.0), Position(8.0, 8.0));
00080
00081 EXPECT_FALSE(iterator.isPastEnd());
00082 EXPECT_EQ(5, (*iterator)(0));
00083 EXPECT_EQ(4, (*iterator)(1));
00084
00085 ++iterator;
00086 EXPECT_FALSE(iterator.isPastEnd());
00087 EXPECT_EQ(4, (*iterator)(0));
00088 EXPECT_EQ(3, (*iterator)(1));
00089
00090 ++iterator;
00091 EXPECT_FALSE(iterator.isPastEnd());
00092 EXPECT_EQ(3, (*iterator)(0));
00093 EXPECT_EQ(2, (*iterator)(1));
00094
00095 ++iterator;
00096 ++iterator;
00097 ++iterator;
00098 EXPECT_TRUE(iterator.isPastEnd());
00099 }
00100
00101 TEST(LineIterator, StartAndEndOutsideMapWithoutIntersectingMap)
00102 {
00103 GridMap map( { "types" });
00104 map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
00105
00106 LineIterator iterator(map, Position(-8.0, 8.0), Position(8.0, 8.0));
00107
00108 EXPECT_TRUE(iterator.isPastEnd());
00109 }
00110
00111 TEST(LineIterator, MovedMap)
00112 {
00113 GridMap map( { "types" });
00114 map.setGeometry(Length(7.0, 5.0), 1.0, Position(0.0, 0.0));
00115 map.move(Position(2.0, 2.0));
00116
00117 LineIterator iterator(map, Position(0.0, 0.0), Position(2.0, 2.0));
00118 Position point;
00119
00120 EXPECT_FALSE(iterator.isPastEnd());
00121 map.getPosition(*iterator, point);
00122 EXPECT_EQ(0, point.x());
00123 EXPECT_EQ(0, point.y());
00124
00125 ++iterator;
00126 EXPECT_FALSE(iterator.isPastEnd());
00127 map.getPosition(*iterator, point);
00128 EXPECT_EQ(1, point.x());
00129 EXPECT_EQ(1, point.y());
00130
00131 ++iterator;
00132 EXPECT_FALSE(iterator.isPastEnd());
00133 map.getPosition(*iterator, point);
00134 EXPECT_EQ(2, point.x());
00135 EXPECT_EQ(2, point.y());
00136
00137 ++iterator;
00138 EXPECT_TRUE(iterator.isPastEnd());
00139 }
00140
00141 TEST(LineIterator, StartAndEndOutsideMovedMap)
00142 {
00143 GridMap map( { "types" });
00144 map.setGeometry(Length(7.0, 5.0), 1.0, Position(0.0, 0.0));
00145 map.move(Position(2.0, 2.0));
00146
00147 LineIterator iterator(map, Position(0.0, 0.0), Position(8.0, 8.0));
00148 Position point;
00149
00150 EXPECT_FALSE(iterator.isPastEnd());
00151 map.getPosition(*iterator, point);
00152 EXPECT_EQ(0, point.x());
00153 EXPECT_EQ(0, point.y());
00154
00155 ++iterator;
00156 map.getPosition(*iterator, point);
00157 EXPECT_EQ(1, point.x());
00158 EXPECT_EQ(1, point.y());
00159
00160 ++iterator;
00161 map.getPosition(*iterator, point);
00162 EXPECT_EQ(2, point.x());
00163 EXPECT_EQ(2, point.y());
00164
00165 ++iterator;
00166 map.getPosition(*iterator, point);
00167 EXPECT_EQ(3, point.x());
00168 EXPECT_EQ(3, point.y());
00169
00170 ++iterator;
00171 map.getPosition(*iterator, point);
00172 EXPECT_EQ(4, point.x());
00173 EXPECT_EQ(4, point.y());
00174
00175 ++iterator;
00176 EXPECT_TRUE(iterator.isPastEnd());
00177 }