00001
00004
00005
00006
00007
00008 #include "../include/cost_map_core/common.hpp"
00009 #include <Eigen/Core>
00010
00011
00012 #include <gtest/gtest.h>
00013
00014
00015 #include <cfloat>
00016
00017
00018 #include <vector>
00019 #include "../include/cost_map_core/cost_map.hpp"
00020 #include <grid_map_core/Polygon.hpp>
00021 #include "../include/cost_map_core/iterators/polygon_iterator.hpp"
00022
00023 using namespace std;
00024 using namespace Eigen;
00025
00026 TEST(PolygonIterator, FullCover)
00027 {
00028 vector<string> types;
00029 types.push_back("type");
00030 cost_map::CostMap map(types);
00031 map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0));
00032
00033 cost_map::Polygon polygon;
00034 polygon.addVertex(cost_map::Position(-100.0, 100.0));
00035 polygon.addVertex(cost_map::Position(100.0, 100.0));
00036 polygon.addVertex(cost_map::Position(100.0, -100.0));
00037 polygon.addVertex(cost_map::Position(-100.0, -100.0));
00038
00039 cost_map::PolygonIterator iterator(map, polygon);
00040
00041 EXPECT_FALSE(iterator.isPastEnd());
00042 EXPECT_EQ(0, (*iterator)(0));
00043 EXPECT_EQ(0, (*iterator)(1));
00044
00045 ++iterator;
00046 EXPECT_FALSE(iterator.isPastEnd());
00047 EXPECT_EQ(0, (*iterator)(0));
00048 EXPECT_EQ(1, (*iterator)(1));
00049
00050 ++iterator;
00051 EXPECT_FALSE(iterator.isPastEnd());
00052 EXPECT_EQ(0, (*iterator)(0));
00053 EXPECT_EQ(2, (*iterator)(1));
00054
00055 for (int i = 0; i < 37; ++i) ++iterator;
00056
00057 EXPECT_FALSE(iterator.isPastEnd());
00058 EXPECT_EQ(7, (*iterator)(0));
00059 EXPECT_EQ(4, (*iterator)(1));
00060
00061 ++iterator;
00062 EXPECT_TRUE(iterator.isPastEnd());
00063 }
00064
00065 TEST(PolygonIterator, Outside)
00066 {
00067 cost_map::CostMap map({"types"});
00068 map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0));
00069
00070 cost_map::Polygon polygon;
00071 polygon.addVertex(cost_map::Position(99.0, 101.0));
00072 polygon.addVertex(cost_map::Position(101.0, 101.0));
00073 polygon.addVertex(cost_map::Position(101.0, 99.0));
00074 polygon.addVertex(cost_map::Position(99.0, 99.0));
00075
00076 cost_map::PolygonIterator iterator(map, polygon);
00077
00078 EXPECT_TRUE(iterator.isPastEnd());
00079 }
00080
00081 TEST(PolygonIterator, Square)
00082 {
00083 cost_map::CostMap map({"types"});
00084 map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0));
00085
00086 cost_map::Polygon polygon;
00087 polygon.addVertex(cost_map::Position(-1.0, 1.5));
00088 polygon.addVertex(cost_map::Position(1.0, 1.5));
00089 polygon.addVertex(cost_map::Position(1.0, -1.5));
00090 polygon.addVertex(cost_map::Position(-1.0, -1.5));
00091
00092 cost_map::PolygonIterator iterator(map, polygon);
00093
00094 EXPECT_FALSE(iterator.isPastEnd());
00095 EXPECT_EQ(3, (*iterator)(0));
00096 EXPECT_EQ(1, (*iterator)(1));
00097
00098 ++iterator;
00099 EXPECT_FALSE(iterator.isPastEnd());
00100 EXPECT_EQ(3, (*iterator)(0));
00101 EXPECT_EQ(2, (*iterator)(1));
00102
00103 ++iterator;
00104 EXPECT_FALSE(iterator.isPastEnd());
00105 EXPECT_EQ(3, (*iterator)(0));
00106 EXPECT_EQ(3, (*iterator)(1));
00107
00108 ++iterator;
00109 EXPECT_FALSE(iterator.isPastEnd());
00110 EXPECT_EQ(4, (*iterator)(0));
00111 EXPECT_EQ(1, (*iterator)(1));
00112
00113 ++iterator;
00114 EXPECT_FALSE(iterator.isPastEnd());
00115 EXPECT_EQ(4, (*iterator)(0));
00116 EXPECT_EQ(2, (*iterator)(1));
00117
00118 ++iterator;
00119 EXPECT_FALSE(iterator.isPastEnd());
00120 EXPECT_EQ(4, (*iterator)(0));
00121 EXPECT_EQ(3, (*iterator)(1));
00122
00123 ++iterator;
00124 EXPECT_TRUE(iterator.isPastEnd());
00125 }
00126
00127 TEST(PolygonIterator, TopLeftTriangle)
00128 {
00129 cost_map::CostMap map({"types"});
00130 map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0));
00131
00132 cost_map::Polygon polygon;
00133 polygon.addVertex(cost_map::Position(-40.1, 20.6));
00134 polygon.addVertex(cost_map::Position(40.1, 20.4));
00135 polygon.addVertex(cost_map::Position(-40.1, -20.6));
00136
00137 cost_map::PolygonIterator iterator(map, polygon);
00138
00139 EXPECT_FALSE(iterator.isPastEnd());
00140 EXPECT_EQ(0, (*iterator)(0));
00141 EXPECT_EQ(0, (*iterator)(1));
00142
00143 ++iterator;
00144 EXPECT_FALSE(iterator.isPastEnd());
00145 EXPECT_EQ(1, (*iterator)(0));
00146 EXPECT_EQ(0, (*iterator)(1));
00147
00148
00149 }
00150
00151 int main(int argc, char **argv)
00152 {
00153 testing::InitGoogleTest(&argc, argv);
00154 srand((int)time(0));
00155 return RUN_ALL_TESTS();
00156 }
00157