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