@@ -1329,13 +1329,13 @@ std::pair<std::vector<lanelet::ConstPoint3d>, bool> getBoundWithFreeSpaceAreas(
1329
1329
};
1330
1330
1331
1331
const auto get_bound_edge = [&ego_pose, &is_driving_freespace, &is_left](
1332
- const auto & bound, const auto is_inside ) {
1332
+ const auto & bound, const auto trim_behind_bound ) {
1333
1333
if (!is_driving_freespace) {
1334
1334
return bound;
1335
1335
}
1336
1336
1337
1337
const auto p_offset = tier4_autoware_utils::calcOffsetPose (
1338
- ego_pose, (is_inside ? -100.0 : 100.0 ), (is_left ? 0.1 : -0.1 ), 0.0 );
1338
+ ego_pose, (trim_behind_bound ? -100.0 : 100.0 ), (is_left ? 0.1 : -0.1 ), 0.0 );
1339
1339
1340
1340
std::vector<lanelet::ConstPoint3d> ret;
1341
1341
for (size_t i = 1 ; i < bound.size (); ++i) {
@@ -1357,37 +1357,63 @@ std::pair<std::vector<lanelet::ConstPoint3d>, bool> getBoundWithFreeSpaceAreas(
1357
1357
1358
1358
std::vector<lanelet::ConstPoint3d> expanded_bound;
1359
1359
1360
- if (original_bound_itr->id () != original_bound_rev_itr->id ()) {
1361
- const auto polygon_bound =
1362
- extract_bound_from_polygon (original_bound_itr->id (), original_bound_rev_itr->id ());
1363
-
1364
- expanded_bound.insert (expanded_bound.end (), original_bound.begin (), original_bound_itr);
1365
- expanded_bound.insert (expanded_bound.end (), polygon_bound.begin (), polygon_bound.end ());
1366
- expanded_bound.insert (
1367
- expanded_bound.end (), std::next (original_bound_rev_itr).base (), original_bound.end ());
1360
+ enum class RouteCase {
1361
+ ROUTE_IS_PASS_THROUGH_FREESPACE = 0 ,
1362
+ GOAL_POS_IS_IN_FREESPACE,
1363
+ INIT_POS_IS_IN_FREESPACE,
1364
+ OTHER,
1365
+ };
1368
1366
1369
- } else if (boost::geometry::within (
1370
- to2D (original_bound.front ().basicPoint ()), to2D (polygon).basicPolygon ())) {
1371
- auto polygon_bound =
1372
- extract_bound_from_polygon (other_side_bound_itr->id (), original_bound_itr->id ());
1373
- std::reverse (polygon_bound.begin (), polygon_bound.end ());
1374
- auto bound_edge = get_bound_edge (polygon_bound, true );
1375
- std::reverse (bound_edge.begin (), bound_edge.end ());
1367
+ const auto route_case = [&]() {
1368
+ if (original_bound_itr->id () != original_bound_rev_itr->id ()) {
1369
+ return RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE;
1370
+ } else if (boost::geometry::within (
1371
+ to2D (original_bound.front ().basicPoint ()), to2D (polygon).basicPolygon ())) {
1372
+ return RouteCase::INIT_POS_IS_IN_FREESPACE;
1373
+ } else if (boost::geometry::within (
1374
+ to2D (original_bound.back ().basicPoint ()), to2D (polygon).basicPolygon ())) {
1375
+ return RouteCase::GOAL_POS_IS_IN_FREESPACE;
1376
+ }
1377
+ return RouteCase::OTHER;
1378
+ }();
1376
1379
1377
- expanded_bound.insert (expanded_bound.end (), bound_edge.begin (), bound_edge.end ());
1378
- expanded_bound.insert (expanded_bound.end (), original_bound_itr, original_bound.end ());
1380
+ switch (route_case) {
1381
+ case RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE: {
1382
+ const auto polygon_bound =
1383
+ extract_bound_from_polygon (original_bound_itr->id (), original_bound_rev_itr->id ());
1379
1384
1380
- } else if (boost::geometry::within (
1381
- to2D (original_bound.back ().basicPoint ()), to2D (polygon).basicPolygon ())) {
1382
- const auto polygon_bound =
1383
- extract_bound_from_polygon (original_bound_itr->id (), other_side_bound_itr->id ());
1384
- const auto bound_edge = get_bound_edge (polygon_bound, false );
1385
+ expanded_bound.insert (expanded_bound.end (), original_bound.begin (), original_bound_itr);
1386
+ expanded_bound.insert (expanded_bound.end (), polygon_bound.begin (), polygon_bound.end ());
1387
+ expanded_bound.insert (
1388
+ expanded_bound.end (), std::next (original_bound_rev_itr).base (), original_bound.end ());
1389
+ break ;
1390
+ }
1391
+ case RouteCase::INIT_POS_IS_IN_FREESPACE: {
1392
+ auto polygon_bound =
1393
+ extract_bound_from_polygon (other_side_bound_itr->id (), original_bound_itr->id ());
1394
+ std::reverse (polygon_bound.begin (), polygon_bound.end ());
1395
+ auto bound_edge = get_bound_edge (polygon_bound, true );
1396
+ std::reverse (bound_edge.begin (), bound_edge.end ());
1385
1397
1386
- expanded_bound.insert (expanded_bound.end (), original_bound.begin (), original_bound_itr);
1387
- expanded_bound.insert (expanded_bound.end (), bound_edge.begin (), bound_edge.end ());
1398
+ expanded_bound.insert (expanded_bound.end (), bound_edge.begin (), bound_edge.end ());
1399
+ expanded_bound.insert (expanded_bound.end (), original_bound_itr, original_bound.end ());
1400
+ break ;
1401
+ }
1402
+ case RouteCase::GOAL_POS_IS_IN_FREESPACE: {
1403
+ const auto polygon_bound =
1404
+ extract_bound_from_polygon (original_bound_itr->id (), other_side_bound_itr->id ());
1405
+ const auto bound_edge = get_bound_edge (polygon_bound, false );
1388
1406
1389
- } else {
1390
- expanded_bound = original_bound;
1407
+ expanded_bound.insert (expanded_bound.end (), original_bound.begin (), original_bound_itr);
1408
+ expanded_bound.insert (expanded_bound.end (), bound_edge.begin (), bound_edge.end ());
1409
+ break ;
1410
+ }
1411
+ case RouteCase::OTHER: {
1412
+ expanded_bound = original_bound;
1413
+ break ;
1414
+ }
1415
+ default :
1416
+ throw std::domain_error (" invalid case." );
1391
1417
}
1392
1418
1393
1419
const auto goal_is_in_freespace = boost::geometry::within (
0 commit comments