00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <gtest/gtest.h>
00038 #include <costmap_2d/costmap_2d.h>
00039
00040 using namespace costmap_2d;
00041
00042 TEST(CostmapCoordinates, easy_coordinates_test)
00043 {
00044 Costmap2D costmap(2, 3, 1.0, 0, 0);
00045
00046 double wx, wy;
00047 costmap.mapToWorld(0u, 0u, wx, wy);
00048 EXPECT_DOUBLE_EQ(wx, 0.5);
00049 EXPECT_DOUBLE_EQ(wy, 0.5);
00050 costmap.mapToWorld(1u, 2u, wx, wy);
00051 EXPECT_DOUBLE_EQ(wx, 1.5);
00052 EXPECT_DOUBLE_EQ(wy, 2.5);
00053
00054 unsigned int umx, umy;
00055 int mx, my;
00056 ASSERT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
00057 EXPECT_EQ(umx, 1);
00058 EXPECT_EQ(umy, 2);
00059 costmap.worldToMapNoBounds(wx, wy, mx, my);
00060 EXPECT_EQ(mx, 1);
00061 EXPECT_EQ(my, 2);
00062
00063
00064 wx = 2.5;
00065 EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
00066 costmap.worldToMapEnforceBounds(wx, wy, mx, my);
00067 EXPECT_EQ(mx, 1);
00068 EXPECT_EQ(my, 2);
00069 costmap.worldToMapNoBounds(wx, wy, mx, my);
00070 EXPECT_EQ(mx, 2);
00071 EXPECT_EQ(my, 2);
00072
00073
00074 EXPECT_TRUE(costmap.worldToMap(0.0, wy, umx, umy));
00075 EXPECT_EQ(umx, 0);
00076 EXPECT_TRUE(costmap.worldToMap(0.25, wy, umx, umy));
00077 EXPECT_EQ(umx, 0);
00078 EXPECT_TRUE(costmap.worldToMap(0.75, wy, umx, umy));
00079 EXPECT_EQ(umx, 0);
00080 EXPECT_TRUE(costmap.worldToMap(0.9999, wy, umx, umy));
00081 EXPECT_EQ(umx, 0);
00082 EXPECT_TRUE(costmap.worldToMap(1.0, wy, umx, umy));
00083 EXPECT_EQ(umx, 1);
00084 EXPECT_TRUE(costmap.worldToMap(1.25, wy, umx, umy));
00085 EXPECT_EQ(umx, 1);
00086 EXPECT_TRUE(costmap.worldToMap(1.75, wy, umx, umy));
00087 EXPECT_EQ(umx, 1);
00088 EXPECT_TRUE(costmap.worldToMap(1.9999, wy, umx, umy));
00089 EXPECT_EQ(umx, 1);
00090 EXPECT_FALSE(costmap.worldToMap(2.0, wy, umx, umy));
00091 costmap.worldToMapEnforceBounds(2.0, wy, mx, my);
00092 EXPECT_EQ(mx, 1);
00093 }
00094
00095 TEST(CostmapCoordinates, hard_coordinates_test)
00096 {
00097 Costmap2D costmap(2, 3, 0.1, -0.2, 0.2);
00098
00099 double wx, wy;
00100 costmap.mapToWorld(0, 0, wx, wy);
00101 EXPECT_DOUBLE_EQ(wx, -0.15);
00102 EXPECT_DOUBLE_EQ(wy, 0.25);
00103 costmap.mapToWorld(1, 2, wx, wy);
00104 EXPECT_DOUBLE_EQ(wx, -0.05);
00105 EXPECT_DOUBLE_EQ(wy, 0.45);
00106
00107 unsigned int umx, umy;
00108 int mx, my;
00109 EXPECT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
00110 EXPECT_EQ(umx, 1);
00111 EXPECT_EQ(umy, 2);
00112 costmap.worldToMapNoBounds(wx, wy, mx, my);
00113 EXPECT_EQ(mx, 1);
00114 EXPECT_EQ(my, 2);
00115
00116
00117 wx = 2.5;
00118 EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
00119 costmap.worldToMapEnforceBounds(wx, wy, mx, my);
00120 EXPECT_EQ(mx, 1);
00121 EXPECT_EQ(my, 2);
00122 costmap.worldToMapNoBounds(wx, wy, mx, my);
00123 EXPECT_EQ(mx, 27);
00124 EXPECT_EQ(my, 2);
00125 }
00126
00127 int main(int argc, char** argv)
00128 {
00129 testing::InitGoogleTest( &argc, argv );
00130 return RUN_ALL_TESTS();
00131 }
00132