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
00035 #include <costmap_2d/costmap_2d.h>
00036 #include <costmap_2d/layered_costmap.h>
00037 #include <costmap_2d/observation_buffer.h>
00038 #include <costmap_2d/testing_helper.h>
00039 #include <set>
00040 #include <gtest/gtest.h>
00041 #include <tf/transform_listener.h>
00042
00043 using namespace costmap_2d;
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00074 TEST(costmap, testRaytracing){
00075 tf::TransformListener tf;
00076
00077 LayeredCostmap layers("frame", false, false);
00078 addStaticLayer(layers, tf);
00079 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00080
00081
00082 addObservation(olayer, 0.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
00083
00084
00085 layers.updateMap(0,0,0);
00086
00087
00088 int lethal_count = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
00089
00090
00091 ASSERT_EQ(lethal_count, 21);
00092 }
00093
00097 TEST(costmap, testRaytracing2){
00098 tf::TransformListener tf;
00099 LayeredCostmap layers("frame", false, false);
00100 addStaticLayer(layers, tf);
00101 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00102
00103
00104
00105
00106
00107 layers.updateMap(0,0,0);
00108
00109
00110
00111
00112
00113 int obs_before = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
00114 ASSERT_EQ(obs_before, 20);
00115
00116
00117
00118
00119 addObservation(olayer, 9.5, 9.5, MAX_Z/2, 0.5, 0.5, MAX_Z/2);
00120 layers.updateMap(0, 0, 0);
00121
00122
00123
00124 int obs_after = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
00125
00126
00127
00128
00129 ASSERT_EQ(obs_after, obs_before + 1);
00130
00131
00132 for(int i = 0; i < olayer->getSizeInCellsY(); ++i)
00133 {
00134 olayer->setCost(i, i, LETHAL_OBSTACLE);
00135 }
00136
00137
00138 layers.updateMap(0, 0, 0);
00139
00140
00141
00142 int with_static = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
00143
00144
00145 ASSERT_EQ(with_static, obs_after);
00146
00147 ASSERT_EQ(79, countValues(*(layers.getCostmap()), costmap_2d::FREE_SPACE));
00148 }
00149
00153 TEST(costmap, testWaveInterference){
00154 tf::TransformListener tf;
00155
00156
00157 LayeredCostmap layers("frame", false, true);
00158 layers.resizeMap(10, 10, 1, 0, 0);
00159 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00160
00161
00162
00163
00164
00165 addObservation(olayer, 3.0, 3.0, MAX_Z);
00166 addObservation(olayer, 5.0, 5.0, MAX_Z);
00167 addObservation(olayer, 7.0, 7.0, MAX_Z);
00168 layers.updateMap(0,0,0);
00169
00170 Costmap2D* costmap = layers.getCostmap();
00171
00172
00173
00174 ASSERT_EQ(3, countValues(*costmap, costmap_2d::LETHAL_OBSTACLE));
00175 ASSERT_EQ(92, countValues(*costmap, costmap_2d::NO_INFORMATION));
00176 ASSERT_EQ(5, countValues(*costmap, costmap_2d::FREE_SPACE));
00177 }
00178
00182 TEST(costmap, testZThreshold){
00183 tf::TransformListener tf;
00184
00185 LayeredCostmap layers("frame", false, true);
00186 layers.resizeMap(10, 10, 1, 0, 0);
00187
00188 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00189
00190
00191 addObservation(olayer, 0.0, 5.0, 0.4);
00192 addObservation(olayer, 1.0, 5.0, 2.2);
00193
00194 layers.updateMap(0,0,0);
00195
00196 Costmap2D* costmap = layers.getCostmap();
00197 ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 1);
00198 }
00199
00200
00204 TEST(costmap, testDynamicObstacles){
00205 tf::TransformListener tf;
00206 LayeredCostmap layers("frame", false, false);
00207 addStaticLayer(layers, tf);
00208
00209 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00210
00211
00212 addObservation(olayer, 0.0, 0.0);
00213 addObservation(olayer, 0.0, 0.0);
00214 addObservation(olayer, 0.0, 0.0);
00215
00216 layers.updateMap(0,0,0);
00217
00218 Costmap2D* costmap = layers.getCostmap();
00219
00220 ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
00221
00222
00223 ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
00224 }
00225
00226
00230 TEST(costmap, testMultipleAdditions){
00231 tf::TransformListener tf;
00232 LayeredCostmap layers("frame", false, false);
00233 addStaticLayer(layers, tf);
00234
00235 ObstacleLayer* olayer = addObstacleLayer(layers, tf);
00236
00237
00238 addObservation(olayer, 9.5, 0.0);
00239 layers.updateMap(0,0,0);
00240 Costmap2D* costmap = layers.getCostmap();
00241
00242
00243 ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 20);
00244
00245 }
00246
00247
00248 int main(int argc, char** argv){
00249 ros::init(argc, argv, "obstacle_tests");
00250 testing::InitGoogleTest(&argc, argv);
00251 return RUN_ALL_TESTS();
00252 }