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 #include <cstddef>
00031 #include <string>
00032 #include <algorithm>
00033
00034 #include <gtest/gtest.h>
00035
00036 #include <ros/ros.h>
00037 #include <nav_msgs/OccupancyGrid.h>
00038 #include <costmap_cspace/costmap_3d.h>
00039
00040 const std::string footprint_str(
00041 "<value><array><data>"
00042 " <value><array><data>"
00043 " <value><double>1.5</double></value>"
00044 " <value><double>0.0</double></value>"
00045 " </data></array></value>"
00046 " <value><array><data>"
00047 " <value><double>-0.5</double></value>"
00048 " <value><double>-0.5</double></value>"
00049 " </data></array></value>"
00050 " <value><array><data>"
00051 " <value><double>-0.5</double></value>"
00052 " <value><double>0.5</double></value>"
00053 " </data></array></value>"
00054 "</data></array></value>");
00055
00056
00057
00058
00059 const char temp_dir[4][2] =
00060 {
00061
00062 -1, 0,
00063 0, -1,
00064 1, 0,
00065 0, 1
00066 };
00067
00068 TEST(Costmap3dLayerFootprint, CSpaceTemplate)
00069 {
00070 costmap_cspace::Costmap3dLayerFootprint cm;
00071
00072
00073 cm.setAngleResolution(4);
00074 cm.setExpansion(0.0, 0.0);
00075 cm.setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00076
00077
00078 int footprint_offset = 0;
00079 XmlRpc::XmlRpcValue footprint_xml;
00080 ASSERT_TRUE(footprint_xml.fromXml(footprint_str, &footprint_offset));
00081 cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
00082
00083
00084 const costmap_cspace::Polygon polygon = cm.getFootprint();
00085 ASSERT_EQ(polygon.v.size(), 3u + 1u);
00086 ASSERT_EQ(polygon.v[0][0], 1.5);
00087 ASSERT_EQ(polygon.v[0][1], 0.0);
00088 ASSERT_EQ(polygon.v[1][0], -0.5);
00089 ASSERT_EQ(polygon.v[1][1], -0.5);
00090 ASSERT_EQ(polygon.v[2][0], -0.5);
00091 ASSERT_EQ(polygon.v[2][1], 0.5);
00092
00093 ASSERT_EQ(polygon.v[3][0], 1.5);
00094 ASSERT_EQ(polygon.v[3][1], 0.0);
00095 ASSERT_EQ(cm.getFootprintRadius(), 1.5);
00096
00097
00098 costmap_cspace_msgs::MapMetaData3D map_info;
00099 map_info.width = 3;
00100 map_info.height = 3;
00101 map_info.angle = 4;
00102 map_info.linear_resolution = 1.0;
00103 map_info.angular_resolution = M_PI / 2.0;
00104 map_info.origin.orientation.w = 1.0;
00105
00106 cm.setMapMetaData(map_info);
00107
00108 ASSERT_EQ(cm.getRangeMax(), static_cast<int>(ceilf(1.5 / 1.0)));
00109
00110 const costmap_cspace::CSpace3Cache& temp = cm.getTemplate();
00111
00112 int x, y, a;
00113 int cx, cy, ca;
00114 temp.getSize(x, y, a);
00115 temp.getCenter(cx, cy, ca);
00116 ASSERT_EQ(x, 2 * 2 + 1);
00117 ASSERT_EQ(y, 2 * 2 + 1);
00118 ASSERT_EQ(a, 4);
00119 ASSERT_EQ(cx, 2);
00120 ASSERT_EQ(cy, 2);
00121 ASSERT_EQ(ca, 0);
00122
00123
00124 for (int k = -ca; k < a - ca; ++k)
00125 {
00126 for (int j = -cy; j < y - cy; ++j)
00127 {
00128 for (int i = -cx; i < x - cx; ++i)
00129 {
00130 if (i == 0 && j == 0)
00131 {
00132 ASSERT_EQ(temp.e(i, j, k), 100);
00133 }
00134 else if (i == temp_dir[k + ca][0] && j == temp_dir[k + ca][1])
00135 {
00136 ASSERT_EQ(temp.e(i, j, k), 100);
00137 }
00138 else
00139 {
00140 ASSERT_EQ(temp.e(i, j, k), 0);
00141 }
00142
00143 }
00144
00145 }
00146
00147 }
00148 }
00149
00150 TEST(Costmap3dLayerPlain, CSpaceTemplate)
00151 {
00152 costmap_cspace::Costmap3dLayerPlain cm;
00153
00154
00155 cm.setAngleResolution(4);
00156 cm.setExpansion(0.0, 0.0);
00157 cm.setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00158
00159
00160 costmap_cspace_msgs::MapMetaData3D map_info;
00161 map_info.width = 1;
00162 map_info.height = 1;
00163 map_info.angle = 4;
00164 map_info.linear_resolution = 1.0;
00165 map_info.angular_resolution = M_PI / 2.0;
00166 map_info.origin.orientation.w = 1.0;
00167
00168 cm.setMapMetaData(map_info);
00169
00170 ASSERT_EQ(cm.getRangeMax(), 0);
00171
00172 const costmap_cspace::CSpace3Cache& temp = cm.getTemplate();
00173
00174 int x, y, a;
00175 int cx, cy, ca;
00176 temp.getSize(x, y, a);
00177 temp.getCenter(cx, cy, ca);
00178 ASSERT_EQ(x, 1);
00179 ASSERT_EQ(y, 1);
00180 ASSERT_EQ(a, 4);
00181 ASSERT_EQ(cx, 0);
00182 ASSERT_EQ(cy, 0);
00183 ASSERT_EQ(ca, 0);
00184
00185
00186 for (int k = -ca; k < a - ca; ++k)
00187 {
00188 ASSERT_EQ(temp.e(0, 0, k), 100);
00189 }
00190 }
00191
00192 TEST(Costmap3dLayerFootprint, CSpaceGenerate)
00193 {
00194 costmap_cspace::Costmap3dLayerFootprint cm;
00195
00196
00197 int footprint_offset = 0;
00198 XmlRpc::XmlRpcValue footprint_xml;
00199 footprint_xml.fromXml(footprint_str, &footprint_offset);
00200 cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
00201
00202
00203 cm.setAngleResolution(4);
00204 cm.setExpansion(0.0, 0.0);
00205 cm.setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00206
00207
00208 nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
00209 map->info.width = 7;
00210 map->info.height = 7;
00211 map->info.resolution = 1.0;
00212 map->info.origin.orientation.w = 1.0;
00213 map->data.resize(map->info.width * map->info.height);
00214
00215
00216 cm.setBaseMap(map);
00217
00218 for (int k = 0; k < cm.getAngularGrid(); ++k)
00219 {
00220 for (size_t j = 0; j < map->info.height; ++j)
00221 {
00222 for (size_t i = 0; i < map->info.width; ++i)
00223 {
00224 const int cost = cm.getMapOverlay()->getCost(i, j, k);
00225
00226 ASSERT_EQ(cost, 0);
00227
00228 }
00229
00230 }
00231
00232 }
00233
00234
00235 for (auto& g : map->data)
00236 {
00237 g = 100;
00238 }
00239 cm.setBaseMap(map);
00240 for (int k = 0; k < cm.getAngularGrid(); ++k)
00241 {
00242 for (size_t j = 0; j < map->info.height; ++j)
00243 {
00244 for (size_t i = 0; i < map->info.width; ++i)
00245 {
00246 const int cost = cm.getMapOverlay()->getCost(i, j, k);
00247
00248 ASSERT_EQ(cost, 100);
00249
00250 }
00251
00252 }
00253
00254 }
00255
00256
00257 for (auto& g : map->data)
00258 {
00259 g = 0;
00260 }
00261 for (size_t i = map->info.width / 2 - 2; i < map->info.width / 2 + 2; ++i)
00262 {
00263 map->data[i + (map->info.height / 2 - 2) * map->info.width] = 100;
00264 map->data[i + (map->info.height / 2 + 2) * map->info.width] = 100;
00265 }
00266 for (size_t i = map->info.height / 2 - 2; i < map->info.height / 2 + 2; ++i)
00267 {
00268 map->data[(map->info.width / 2 - 2) + i * map->info.width] = 100;
00269 }
00270 cm.setBaseMap(map);
00271 for (int k = 0; k < cm.getAngularGrid(); ++k)
00272 {
00273 for (size_t j = 0; j < map->info.height; ++j)
00274 {
00275 for (size_t i = 0; i < map->info.width; ++i)
00276 {
00277 const int cost = cm.getMapOverlay()->getCost(i, j, k);
00278
00279
00280 int cost_offset = 0;
00281 const int i_offset = static_cast<int>(i) - temp_dir[k][0];
00282 const int j_offset = static_cast<int>(j) - temp_dir[k][1];
00283 if (static_cast<size_t>(i_offset) < map->info.width &&
00284 static_cast<size_t>(j_offset) < map->info.height)
00285 {
00286 cost_offset = map->data[i_offset + j_offset * map->info.width];
00287 }
00288 if (map->data[i + j * map->info.width] == 100 || cost_offset == 100)
00289 {
00290 ASSERT_EQ(cost, 100);
00291 }
00292 else
00293 {
00294 ASSERT_EQ(cost, 0);
00295 }
00296
00297 }
00298
00299 }
00300
00301 }
00302 }
00303
00304 TEST(Costmap3dLayerFootprint, CSpaceExpandSpread)
00305 {
00306 costmap_cspace::Costmap3dLayerFootprint cm;
00307
00308
00309 int footprint_offset = 0;
00310 XmlRpc::XmlRpcValue footprint_xml;
00311 footprint_xml.fromXml(footprint_str, &footprint_offset);
00312 cm.setFootprint(costmap_cspace::Polygon(footprint_xml));
00313
00314
00315 const float expand = 1.0;
00316 const float spread = 2.0;
00317 cm.setAngleResolution(4);
00318 cm.setExpansion(expand, spread);
00319 cm.setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00320
00321
00322 nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
00323 map->info.width = 9;
00324 map->info.height = 9;
00325 map->info.resolution = 1.0;
00326 map->info.origin.orientation.w = 1.0;
00327 map->data.resize(map->info.width * map->info.height);
00328
00329 const int max_cost = 80;
00330 map->data[map->info.width / 2 + (map->info.height / 2) * map->info.width] = max_cost;
00331 cm.setBaseMap(map);
00332
00333 for (int k = 0; k < cm.getAngularGrid(); ++k)
00334 {
00335 const int i_center = map->info.width / 2;
00336 const int j_center = map->info.height / 2;
00337 const int i_center2 = map->info.width / 2 + temp_dir[k][0];
00338 const int j_center2 = map->info.height / 2 + temp_dir[k][1];
00339
00340 for (size_t j = 0; j < map->info.height; ++j)
00341 {
00342 for (size_t i = 0; i < map->info.width; ++i)
00343 {
00344 const int cost = cm.getMapOverlay()->getCost(i, j, k);
00345 const float dist1 = hypotf(static_cast<int>(i) - i_center, static_cast<int>(j) - j_center);
00346 const float dist2 = hypotf(static_cast<int>(i) - i_center2, static_cast<int>(j) - j_center2);
00347 const float dist = std::min(dist1, dist2);
00348 if (dist <= expand)
00349 {
00350
00351 EXPECT_EQ(cost, max_cost);
00352 }
00353 else if (dist <= expand + spread)
00354 {
00355
00356 EXPECT_NE(cost, 0);
00357 EXPECT_NE(cost, 100);
00358 }
00359 else if (dist > expand + spread + 1)
00360 {
00361
00362
00363
00364 EXPECT_EQ(cost, 0);
00365 }
00366
00367 }
00368
00369 }
00370
00371 }
00372 }
00373
00374 TEST(Costmap3dLayerFootprint, CSpaceOverwrite)
00375 {
00376 costmap_cspace::Costmap3dLayerFootprint cm_ref;
00377 costmap_cspace::Costmap3dLayerFootprint cm_base;
00378
00379
00380 int footprint_offset = 0;
00381 XmlRpc::XmlRpcValue footprint_xml;
00382 footprint_xml.fromXml(footprint_str, &footprint_offset);
00383 costmap_cspace::Polygon footprint(footprint_xml);
00384 cm_ref.setFootprint(footprint);
00385 cm_base.setFootprint(footprint);
00386
00387
00388 costmap_cspace::Costmap3d cms(4);
00389 auto cm = cms.addRootLayer<costmap_cspace::Costmap3dLayerFootprint>();
00390 cm->setExpansion(0.0, 0.0);
00391 cm->setFootprint(footprint);
00392 auto cm_over = cms.addLayer<costmap_cspace::Costmap3dLayerFootprint>(
00393 costmap_cspace::MapOverlayMode::OVERWRITE);
00394 cm_over->setExpansion(0.0, 0.0);
00395 cm_over->setFootprint(footprint);
00396 auto cm_output = cms.addLayer<costmap_cspace::Costmap3dLayerOutput>();
00397
00398 cm_ref.setAngleResolution(4);
00399 cm_ref.setExpansion(0.0, 0.0);
00400 cm_ref.setOverlayMode(costmap_cspace::MapOverlayMode::OVERWRITE);
00401 cm_base.setAngleResolution(4);
00402 cm_base.setExpansion(0.0, 0.0);
00403 cm_base.setOverlayMode(costmap_cspace::MapOverlayMode::OVERWRITE);
00404
00405
00406 nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
00407 map->info.width = 9;
00408 map->info.height = 9;
00409 map->info.resolution = 1.0;
00410 map->info.origin.orientation.w = 1.0;
00411 map->data.resize(map->info.width * map->info.height);
00412
00413 nav_msgs::OccupancyGrid::Ptr map2(new nav_msgs::OccupancyGrid);
00414 *map2 = *map;
00415
00416 const int num_points_base_map = 2;
00417 const int points_base_map[num_points_base_map][2] =
00418 {
00419 2, 3,
00420 4, 4
00421 };
00422 for (int i = 0; i < num_points_base_map; ++i)
00423 {
00424 map->data[points_base_map[i][0] + points_base_map[i][1] * map->info.width] = 100;
00425 }
00426
00427 const int num_points_local_map = 3;
00428 const int points_local_map[num_points_local_map][2] =
00429 {
00430 3, 4,
00431 5, 3,
00432 4, 4
00433 };
00434 for (int i = 0; i < num_points_local_map; ++i)
00435 {
00436 map2->data[points_local_map[i][0] + points_local_map[i][1] * map->info.width] = 100;
00437 }
00438
00439
00440 cm->setBaseMap(map);
00441
00442
00443 costmap_cspace_msgs::CSpace3DUpdate::Ptr updated(new costmap_cspace_msgs::CSpace3DUpdate);
00444 auto cb = [&updated](
00445 const costmap_cspace::CSpace3DMsg::Ptr map,
00446 const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
00447 {
00448 updated = update;
00449 return true;
00450 };
00451 cm_output->setHandler(cb);
00452 cm_over->processMapOverlay(map2);
00453
00454
00455 ASSERT_EQ(updated->x, 0u);
00456 ASSERT_EQ(updated->y, 0u);
00457 ASSERT_EQ(updated->yaw, 0u);
00458 ASSERT_EQ(updated->width, map->info.width);
00459 ASSERT_EQ(updated->height, map->info.height);
00460 ASSERT_EQ(updated->angle, static_cast<size_t>(cm_over->getAngularGrid()));
00461
00462
00463 cm_ref.setBaseMap(map2);
00464 cm_base.setBaseMap(map);
00465
00466
00467
00468 for (int k = 0; k < cm_over->getAngularGrid(); ++k)
00469 {
00470 for (size_t j = cm_over->getRangeMax(); j < map->info.height - cm_over->getRangeMax(); ++j)
00471 {
00472 for (size_t i = cm_over->getRangeMax(); i < map->info.width - cm_over->getRangeMax(); ++i)
00473 {
00474 const size_t addr = ((k * map->info.height + j) * map->info.width) + i;
00475 ROS_ASSERT(addr < updated->data.size());
00476 const int cost = updated->data[addr];
00477 const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
00478
00479 ASSERT_EQ(cost, cost_ref);
00480
00481
00482 }
00483
00484
00485
00486
00487
00488
00489
00490
00491 }
00492
00493 }
00494
00495 cm_over->setAngleResolution(4);
00496 cm_over->setExpansion(0.0, 0.0);
00497 cm_over->setOverlayMode(costmap_cspace::MapOverlayMode::MAX);
00498 costmap_cspace_msgs::CSpace3DUpdate::Ptr updated_max(new costmap_cspace_msgs::CSpace3DUpdate);
00499 auto cb_max = [&updated_max](
00500 const costmap_cspace::CSpace3DMsg::Ptr map,
00501 const costmap_cspace_msgs::CSpace3DUpdate::Ptr& update) -> bool
00502 {
00503 updated_max = update;
00504 return true;
00505 };
00506 cm_output->setHandler(cb_max);
00507 cm_over->processMapOverlay(map2);
00508
00509 ASSERT_EQ(updated_max->x, 0u);
00510 ASSERT_EQ(updated_max->y, 0u);
00511 ASSERT_EQ(updated_max->yaw, 0u);
00512 ASSERT_EQ(updated_max->width, map->info.width);
00513 ASSERT_EQ(updated_max->height, map->info.height);
00514 ASSERT_EQ(updated_max->angle, static_cast<size_t>(cm_over->getAngularGrid()));
00515
00516 for (int k = 0; k < cm_over->getAngularGrid(); ++k)
00517 {
00518 for (int j = cm_over->getRangeMax(); j < static_cast<int>(map->info.height) - cm_over->getRangeMax(); ++j)
00519 {
00520 for (int i = cm_over->getRangeMax(); i < static_cast<int>(map->info.width) - cm_over->getRangeMax(); ++i)
00521 {
00522 const size_t addr = ((k * map->info.height + j) * map->info.width) + i;
00523 ROS_ASSERT(addr < updated_max->data.size());
00524 const int cost = updated_max->data[addr];
00525 const int cost_ref = cm_ref.getMapOverlay()->getCost(i, j, k);
00526 const int cost_base = cm_base.getMapOverlay()->getCost(i, j, k);
00527 const int cost_max = std::max(cost_ref, cost_base);
00528
00529 ASSERT_EQ(cost, cost_max);
00530
00531
00532 }
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548 }
00549
00550 }
00551 }
00552
00553 TEST(Costmap3dLayerFootprint, CSpaceOverlayMove)
00554 {
00555
00556 int footprint_offset = 0;
00557 XmlRpc::XmlRpcValue footprint_xml;
00558 footprint_xml.fromXml(footprint_str, &footprint_offset);
00559 costmap_cspace::Polygon footprint(footprint_xml);
00560
00561
00562 costmap_cspace::Costmap3d cms(4);
00563 auto cm = cms.addRootLayer<costmap_cspace::Costmap3dLayerFootprint>();
00564 cm->setExpansion(0.0, 0.0);
00565 cm->setFootprint(footprint);
00566 auto cm_over = cms.addLayer<costmap_cspace::Costmap3dLayerFootprint>(
00567 costmap_cspace::MapOverlayMode::MAX);
00568 cm_over->setExpansion(0.0, 0.0);
00569 cm_over->setFootprint(footprint);
00570
00571
00572 nav_msgs::OccupancyGrid::Ptr map(new nav_msgs::OccupancyGrid);
00573 map->info.width = 5;
00574 map->info.height = 5;
00575 map->info.resolution = 1.0;
00576 map->info.origin.orientation.w = 1.0;
00577 map->data.resize(map->info.width * map->info.height);
00578
00579 const int max_cost = 100;
00580 map->data[map->info.width / 2 + (map->info.height / 2) * map->info.width] = max_cost;
00581 cm->setBaseMap(map);
00582
00583
00584 nav_msgs::OccupancyGrid::Ptr map2(new nav_msgs::OccupancyGrid);
00585 *map2 = *map;
00586
00587 for (int xp = -1; xp <= 1; ++xp)
00588 {
00589 for (int yp = -1; yp <= 1; ++yp)
00590 {
00591 map2->info.origin.position.x = map2->info.resolution * xp;
00592 map2->info.origin.position.y = map2->info.resolution * yp;
00593
00594
00595
00596
00597
00598 cm_over->processMapOverlay(map2);
00599 for (int k = 0; k < cm_over->getAngularGrid(); ++k)
00600 {
00601 const size_t i_center = map->info.width / 2;
00602 const size_t j_center = map->info.height / 2;
00603 const size_t i_center2 = map->info.width / 2 + temp_dir[k][0];
00604 const size_t j_center2 = map->info.height / 2 + temp_dir[k][1];
00605
00606 for (size_t j = 0; j < map->info.height; ++j)
00607 {
00608 for (size_t i = 0; i < map->info.width; ++i)
00609 {
00610 const int cost = cm_over->getMapOverlay()->getCost(i, j, k);
00611
00612 if ((i == i_center && j == j_center) ||
00613 (i == i_center2 && j == j_center2) ||
00614 (i == i_center + xp && j == j_center + yp) ||
00615 (i == i_center2 + xp && j == j_center2 + yp))
00616 {
00617 ASSERT_EQ(cost, max_cost);
00618 }
00619 else
00620 {
00621 ASSERT_EQ(cost, 0);
00622 }
00623
00624 }
00625
00626 }
00627
00628 }
00629 }
00630 }
00631 }
00632
00633 int main(int argc, char** argv)
00634 {
00635 testing::InitGoogleTest(&argc, argv);
00636
00637 return RUN_ALL_TESTS();
00638 }