00001
00004
00005
00006
00007
00008 #include <gtest/gtest.h>
00009
00010
00011 #include <math.h>
00012 #include "../include/cost_map_core/cost_map.hpp"
00013
00014 using namespace std;
00015 using namespace cost_map;
00016
00017 TEST(CostMap, Move)
00018 {
00019 CostMap map;
00020 map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0));
00021 map.add("layer", 0.0);
00022 map.setBasicLayers(map.getLayers());
00023 std::vector<BufferRegion> regions;
00024 map.move(Position(-3.0, -2.0), regions);
00025 Index startIndex = map.getStartIndex();
00026
00027 EXPECT_EQ(3, startIndex(0));
00028 EXPECT_EQ(2, startIndex(1));
00029
00030
00031 EXPECT_FALSE(map.isValid(Index(0, 0)));
00032 EXPECT_TRUE(map.isValid(Index(3, 2)));
00033 EXPECT_FALSE(map.isValid(Index(2, 2)));
00034 EXPECT_FALSE(map.isValid(Index(3, 1)));
00035 EXPECT_TRUE(map.isValid(Index(7, 4)));
00036
00037 EXPECT_EQ(2, regions.size());
00038 EXPECT_EQ(0, regions[0].getStartIndex()[0]);
00039 EXPECT_EQ(0, regions[0].getStartIndex()[1]);
00040 EXPECT_EQ(3, regions[0].getSize()[0]);
00041 EXPECT_EQ(5, regions[0].getSize()[1]);
00042 EXPECT_EQ(0, regions[1].getStartIndex()[0]);
00043 EXPECT_EQ(0, regions[1].getStartIndex()[1]);
00044 EXPECT_EQ(8, regions[1].getSize()[0]);
00045 EXPECT_EQ(2, regions[1].getSize()[1]);
00046 }
00047
00048
00049 TEST(AddDataFrom, extendMapAligned)
00050 {
00051 CostMap map1, map2;
00052 map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0));
00053 map1.add("zero", 0);
00054 map1.add("one", 1);
00055 map1.setBasicLayers(map1.getLayers());
00056
00057 map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
00058 map2.add("one", 2);
00059 map2.add("two", 3);
00060 map2.setBasicLayers(map1.getLayers());
00061
00062 EXPECT_FALSE(map1.isInside(Position(3.0, 3.0)));
00063
00064 map1.addDataFrom(map2, true, true, true);
00065
00066 EXPECT_TRUE(map1.exists("two"));
00067 EXPECT_TRUE(map1.isInside(Position(3.0, 3.0)));
00068 EXPECT_DOUBLE_EQ(6.0, map1.getLength().x());
00069 EXPECT_DOUBLE_EQ(6.0, map1.getLength().y());
00070 EXPECT_DOUBLE_EQ(0.5, map1.getPosition().x());
00071 EXPECT_DOUBLE_EQ(0.5, map1.getPosition().y());
00072 EXPECT_EQ(2, static_cast<int>(map1.atPosition("one", Position(2, 2))));
00073 EXPECT_EQ(1, static_cast<int>(map1.atPosition("one", Position(-2, -2))));
00074 EXPECT_EQ(0, static_cast<int>(map1.atPosition("zero", Position(0.0, 0.0))));
00075 }
00076
00077 TEST(AddDataFrom, extendMapNotAligned)
00078 {
00079 CostMap map1, map2;
00080 map1.setGeometry(Length(6.1, 6.1), 1.0, Position(0.0, 0.0));
00081 map1.add("no_information");
00082 map1.add("one", 1);
00083 map1.add("zero", 0);
00084 map1.setBasicLayers(map1.getLayers());
00085
00086 map2.setGeometry(Length(3.1, 3.1), 1.0, Position(3.2, 3.2));
00087 map2.add("no_information", 1);
00088 map2.add("one", 1);
00089 map2.add("two", 2);
00090 map2.setBasicLayers(map1.getLayers());
00091
00092 std::vector<std::string> stringVector;
00093 stringVector.push_back("no_information");
00094 map1.addDataFrom(map2, true, false, false, stringVector);
00095 Index index;
00096 map1.getIndex(Position(-2, -2), index);
00097
00098 EXPECT_FALSE(map1.exists("two"));
00099 EXPECT_TRUE(map1.isInside(Position(4.0, 4.0)));
00100 EXPECT_DOUBLE_EQ(8.0, map1.getLength().x());
00101 EXPECT_DOUBLE_EQ(8.0, map1.getLength().y());
00102 EXPECT_DOUBLE_EQ(1.0, map1.getPosition().x());
00103 EXPECT_DOUBLE_EQ(1.0, map1.getPosition().y());
00104 EXPECT_FALSE(map1.isValid(index, "no_information"));
00105 EXPECT_DOUBLE_EQ(1, static_cast<int>(map1.atPosition("one", Position(0.0, 0.0))));
00106 EXPECT_DOUBLE_EQ(1, static_cast<int>(map1.atPosition("no_information", Position(3.0, 3.0))));
00107 }
00108
00109 TEST(AddDataFrom, copyData)
00110 {
00111 CostMap map1, map2;
00112 map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0));
00113 map1.add("zero", 0.0);
00114 map1.add("one");
00115 map1.setBasicLayers(map1.getLayers());
00116
00117 map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
00118 map2.add("one", 1.0);
00119 map2.add("two", 2.0);
00120 map2.setBasicLayers(map1.getLayers());
00121
00122 map1.addDataFrom(map2, false, false, true);
00123 Index index;
00124 map1.getIndex(Position(-2, -2), index);
00125
00126 EXPECT_TRUE(map1.exists("two"));
00127 EXPECT_FALSE(map1.isInside(Position(3.0, 3.0)));
00128 EXPECT_DOUBLE_EQ(5.0, map1.getLength().x());
00129 EXPECT_DOUBLE_EQ(5.0, map1.getLength().y());
00130 EXPECT_DOUBLE_EQ(0.0, map1.getPosition().x());
00131 EXPECT_DOUBLE_EQ(0.0, map1.getPosition().y());
00132 EXPECT_DOUBLE_EQ(1, static_cast<int>(map1.atPosition("one", Position(2, 2))));
00133 EXPECT_FALSE(map1.isValid(index, "one"));
00134 EXPECT_DOUBLE_EQ(0, static_cast<int>(map1.atPosition("zero", Position(0.0, 0.0))));
00135 }
00136
00137 int main(int argc, char **argv)
00138 {
00139 testing::InitGoogleTest(&argc, argv);
00140 srand((int)time(0));
00141 return RUN_ALL_TESTS();
00142 }