From 19c7cb7efced1a34611fbe6c70fcc65059bef834 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 8 Oct 2024 11:16:53 +0900 Subject: [PATCH] feat(out_of_lane): ignore lanelets beyond the last path point (#1554) * feat(out_of_lane): ignore lanelets beyond the last path point Signed-off-by: Maxime CLEMENT * style(pre-commit): autofix --------- Signed-off-by: Maxime CLEMENT Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/lanelets_selection.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 738ea22106b29..25bc2a9718de3 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -93,6 +93,16 @@ lanelet::ConstLanelets calculate_ignored_lanelets( const auto is_path_lanelet = contains_lanelet(path_lanelets, l.second.id()); if (!is_path_lanelet) ignored_lanelets.push_back(l.second); } + // ignore lanelets beyond the last path pose + const auto beyond = planning_utils::calculateOffsetPoint2d( + ego_data.path.points.back().point.pose, params.front_offset, 0.0); + const lanelet::BasicPoint2d beyond_point(beyond.x(), beyond.y()); + const auto beyond_lanelets = lanelet::geometry::findWithin2d( + route_handler.getLaneletMapPtr()->laneletLayer, beyond_point, 0.0); + for (const auto & l : beyond_lanelets) { + const auto is_path_lanelet = contains_lanelet(path_lanelets, l.second.id()); + if (!is_path_lanelet) ignored_lanelets.push_back(l.second); + } return ignored_lanelets; }