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