00001
00004
00005
00006
00007
00008 #include "../include/cost_map_core/iterators/line_iterator.hpp"
00009 #include "../include/cost_map_core/cost_map.hpp"
00010
00011
00012 #include <gtest/gtest.h>
00013
00014
00015 #include <cfloat>
00016
00017
00018
00019
00020
00021 using namespace cost_map;
00022
00023
00024
00025
00026
00027 TEST(LineIterator, StartOutsideMap)
00028 {
00029 CostMap map( { "types" });
00030 map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
00031
00032 LineIterator iterator(map, Position(2.0, 2.0), Position(0.0, 0.0));
00033
00034 EXPECT_FALSE(iterator.isPastEnd());
00035 EXPECT_EQ(2, (*iterator)(0));
00036 EXPECT_EQ(0, (*iterator)(1));
00037
00038 ++iterator;
00039 EXPECT_FALSE(iterator.isPastEnd());
00040 EXPECT_EQ(3, (*iterator)(0));
00041 EXPECT_EQ(1, (*iterator)(1));
00042
00043 ++iterator;
00044 EXPECT_FALSE(iterator.isPastEnd());
00045 EXPECT_EQ(4, (*iterator)(0));
00046 EXPECT_EQ(2, (*iterator)(1));
00047
00048 ++iterator;
00049 EXPECT_TRUE(iterator.isPastEnd());
00050 }
00051
00052 TEST(LineIterator, EndOutsideMap)
00053 {
00054 CostMap map( { "types" });
00055 map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
00056
00057 LineIterator iterator(map, Position(0.0, 0.0), Position(9.0, 6.0));
00058
00059 EXPECT_FALSE(iterator.isPastEnd());
00060 EXPECT_EQ(4, (*iterator)(0));
00061 EXPECT_EQ(2, (*iterator)(1));
00062
00063 ++iterator;
00064 EXPECT_FALSE(iterator.isPastEnd());
00065 EXPECT_EQ(3, (*iterator)(0));
00066 EXPECT_EQ(1, (*iterator)(1));
00067
00068 ++iterator;
00069 EXPECT_FALSE(iterator.isPastEnd());
00070 EXPECT_EQ(2, (*iterator)(0));
00071 EXPECT_EQ(1, (*iterator)(1));
00072
00073 ++iterator;
00074 EXPECT_FALSE(iterator.isPastEnd());
00075 ++iterator;
00076 EXPECT_FALSE(iterator.isPastEnd());
00077 ++iterator;
00078 EXPECT_TRUE(iterator.isPastEnd());
00079 }
00080
00081 TEST(LineIterator, StartAndEndOutsideMap)
00082 {
00083 CostMap map( { "types" });
00084 map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
00085
00086 LineIterator iterator(map, Position(-7.0, -9.0), Position(8.0, 8.0));
00087
00088 EXPECT_FALSE(iterator.isPastEnd());
00089 EXPECT_EQ(5, (*iterator)(0));
00090 EXPECT_EQ(4, (*iterator)(1));
00091
00092 ++iterator;
00093 EXPECT_FALSE(iterator.isPastEnd());
00094 EXPECT_EQ(4, (*iterator)(0));
00095 EXPECT_EQ(3, (*iterator)(1));
00096
00097 ++iterator;
00098 EXPECT_FALSE(iterator.isPastEnd());
00099 EXPECT_EQ(3, (*iterator)(0));
00100 EXPECT_EQ(2, (*iterator)(1));
00101
00102 ++iterator;
00103 ++iterator;
00104 ++iterator;
00105 EXPECT_TRUE(iterator.isPastEnd());
00106 }
00107
00108 TEST(LineIterator, StartAndEndOutsideMapWithoutIntersectingMap)
00109 {
00110 CostMap map( { "types" });
00111 map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
00112
00113 LineIterator iterator(map, Position(-8.0, 8.0), Position(8.0, 8.0));
00114
00115 EXPECT_TRUE(iterator.isPastEnd());
00116 }
00117
00118
00119
00120
00121
00122 int main(int argc, char **argv)
00123 {
00124 testing::InitGoogleTest(&argc, argv);
00125 srand((int)time(0));
00126 return RUN_ALL_TESTS();
00127 }
00128