test/polygon_iterator.cpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Includes
6 *****************************************************************************/
7 
8 #include "../include/cost_map_core/common.hpp"
9 #include <Eigen/Core>
10 
11 // gtest
12 #include <gtest/gtest.h>
13 
14 // Limits
15 #include <cfloat>
16 
17 // Vector
18 #include <vector>
19 #include "../include/cost_map_core/cost_map.hpp"
20 #include <grid_map_core/Polygon.hpp>
21 #include "../include/cost_map_core/iterators/polygon_iterator.hpp"
22 
23 using namespace std;
24 using namespace Eigen;
25 
26 TEST(PolygonIterator, FullCover)
27 {
28  vector<string> types;
29  types.push_back("type");
30  cost_map::CostMap map(types);
31  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0)); // bufferSize(8, 5)
32 
33  cost_map::Polygon polygon;
34  polygon.addVertex(cost_map::Position(-100.0, 100.0));
35  polygon.addVertex(cost_map::Position(100.0, 100.0));
36  polygon.addVertex(cost_map::Position(100.0, -100.0));
37  polygon.addVertex(cost_map::Position(-100.0, -100.0));
38 
39  cost_map::PolygonIterator iterator(map, polygon);
40 
41  EXPECT_FALSE(iterator.isPastEnd());
42  EXPECT_EQ(0, (*iterator)(0));
43  EXPECT_EQ(0, (*iterator)(1));
44 
45  ++iterator;
46  EXPECT_FALSE(iterator.isPastEnd());
47  EXPECT_EQ(0, (*iterator)(0));
48  EXPECT_EQ(1, (*iterator)(1));
49 
50  ++iterator;
51  EXPECT_FALSE(iterator.isPastEnd());
52  EXPECT_EQ(0, (*iterator)(0));
53  EXPECT_EQ(2, (*iterator)(1));
54 
55  for (int i = 0; i < 37; ++i) ++iterator;
56 
57  EXPECT_FALSE(iterator.isPastEnd());
58  EXPECT_EQ(7, (*iterator)(0));
59  EXPECT_EQ(4, (*iterator)(1));
60 
61  ++iterator;
62  EXPECT_TRUE(iterator.isPastEnd());
63 }
64 
65 TEST(PolygonIterator, Outside)
66 {
67  cost_map::CostMap map({"types"});
68  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0)); // bufferSize(8, 5)
69 
70  cost_map::Polygon polygon;
71  polygon.addVertex(cost_map::Position(99.0, 101.0));
72  polygon.addVertex(cost_map::Position(101.0, 101.0));
73  polygon.addVertex(cost_map::Position(101.0, 99.0));
74  polygon.addVertex(cost_map::Position(99.0, 99.0));
75 
76  cost_map::PolygonIterator iterator(map, polygon);
77 
78  EXPECT_TRUE(iterator.isPastEnd());
79 }
80 
81 TEST(PolygonIterator, Square)
82 {
83  cost_map::CostMap map({"types"});
84  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0)); // bufferSize(8, 5)
85 
86  cost_map::Polygon polygon;
87  polygon.addVertex(cost_map::Position(-1.0, 1.5));
88  polygon.addVertex(cost_map::Position(1.0, 1.5));
89  polygon.addVertex(cost_map::Position(1.0, -1.5));
90  polygon.addVertex(cost_map::Position(-1.0, -1.5));
91 
92  cost_map::PolygonIterator iterator(map, polygon);
93 
94  EXPECT_FALSE(iterator.isPastEnd());
95  EXPECT_EQ(3, (*iterator)(0));
96  EXPECT_EQ(1, (*iterator)(1));
97 
98  ++iterator;
99  EXPECT_FALSE(iterator.isPastEnd());
100  EXPECT_EQ(3, (*iterator)(0));
101  EXPECT_EQ(2, (*iterator)(1));
102 
103  ++iterator;
104  EXPECT_FALSE(iterator.isPastEnd());
105  EXPECT_EQ(3, (*iterator)(0));
106  EXPECT_EQ(3, (*iterator)(1));
107 
108  ++iterator;
109  EXPECT_FALSE(iterator.isPastEnd());
110  EXPECT_EQ(4, (*iterator)(0));
111  EXPECT_EQ(1, (*iterator)(1));
112 
113  ++iterator;
114  EXPECT_FALSE(iterator.isPastEnd());
115  EXPECT_EQ(4, (*iterator)(0));
116  EXPECT_EQ(2, (*iterator)(1));
117 
118  ++iterator;
119  EXPECT_FALSE(iterator.isPastEnd());
120  EXPECT_EQ(4, (*iterator)(0));
121  EXPECT_EQ(3, (*iterator)(1));
122 
123  ++iterator;
124  EXPECT_TRUE(iterator.isPastEnd());
125 }
126 
127 TEST(PolygonIterator, TopLeftTriangle)
128 {
129  cost_map::CostMap map({"types"});
130  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0)); // bufferSize(8, 5)
131 
132  cost_map::Polygon polygon;
133  polygon.addVertex(cost_map::Position(-40.1, 20.6));
134  polygon.addVertex(cost_map::Position(40.1, 20.4));
135  polygon.addVertex(cost_map::Position(-40.1, -20.6));
136 
137  cost_map::PolygonIterator iterator(map, polygon);
138 
139  EXPECT_FALSE(iterator.isPastEnd());
140  EXPECT_EQ(0, (*iterator)(0));
141  EXPECT_EQ(0, (*iterator)(1));
142 
143  ++iterator;
144  EXPECT_FALSE(iterator.isPastEnd());
145  EXPECT_EQ(1, (*iterator)(0));
146  EXPECT_EQ(0, (*iterator)(1));
147 
148  // TODO Extend.
149 }
150 
151 int main(int argc, char **argv)
152 {
153  testing::InitGoogleTest(&argc, argv);
154  srand((int)time(0));
155  return RUN_ALL_TESTS();
156 }
157 
cost_map::PolygonIterator
Definition: polygon_iterator.hpp:19
cost_map::PolygonIterator::isPastEnd
bool isPastEnd() const
Definition: src/lib/iterators/polygon_iterator.cpp:65
TEST
TEST(PolygonIterator, FullCover)
Definition: test/polygon_iterator.cpp:26
cost_map::CostMap::setGeometry
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
Definition: src/lib/cost_map.cpp:43
grid_map::Polygon
main
int main(int argc, char **argv)
Definition: test/polygon_iterator.cpp:151
grid_map::Polygon::addVertex
void addVertex(const Position &vertex)
cost_map::CostMap
Definition: cost_map.hpp:34
cost_map::Position
grid_map::Position Position
Definition: common.hpp:41
cost_map::Length
grid_map::Length Length
Definition: common.hpp:47

cost_map_core
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:03:41