Skip to content

Commit 1ea4dfa

Browse files
committed
refactor(static_drivable_area_expansion): improve readability
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 9dbcbd7 commit 1ea4dfa

File tree

1 file changed

+54
-28
lines changed

1 file changed

+54
-28
lines changed

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

+54-28
Original file line numberDiff line numberDiff line change
@@ -1329,13 +1329,13 @@ std::pair<std::vector<lanelet::ConstPoint3d>, bool> getBoundWithFreeSpaceAreas(
13291329
};
13301330

13311331
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) {
13331333
if (!is_driving_freespace) {
13341334
return bound;
13351335
}
13361336

13371337
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);
13391339

13401340
std::vector<lanelet::ConstPoint3d> ret;
13411341
for (size_t i = 1; i < bound.size(); ++i) {
@@ -1357,37 +1357,63 @@ std::pair<std::vector<lanelet::ConstPoint3d>, bool> getBoundWithFreeSpaceAreas(
13571357

13581358
std::vector<lanelet::ConstPoint3d> expanded_bound;
13591359

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+
};
13681366

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+
}();
13761379

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());
13791384

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());
13851397

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);
13881406

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.");
13911417
}
13921418

13931419
const auto goal_is_in_freespace = boost::geometry::within(

0 commit comments

Comments
 (0)