We read every piece of feedback, and take your input very seriously.
To see all available qualifiers, see our documentation.
1 parent 7d50b6e commit 1d1b3ecCopy full SHA for 1d1b3ec
planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
@@ -715,13 +715,8 @@ lanelet::Lanelet createDepartureCheckLanelet(
715
lanelet::Points3d points;
716
const auto & bound = left_side_parking ? (is_outer ? lane.leftBound() : lane.rightBound())
717
: (is_outer ? lane.rightBound() : lane.leftBound());
718
- if (invert) {
719
- for (const auto & pt : bound.invert()) {
720
- points.push_back(lanelet::Point3d(pt));
721
- }
722
- return points;
723
724
- for (const auto & pt : bound) {
+ const auto ego_oriented_bound = invert ? bound.invert() : bound;
+ for (const auto & pt : ego_oriented_bound) {
725
points.push_back(lanelet::Point3d(pt));
726
}
727
return points;
0 commit comments