From 8574fb95ca1e671d8df43425a54536a2605a78af Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 25 Sep 2024 17:40:40 +0900 Subject: [PATCH] feat(out_of_lane): ignore lanelets beyond the last path point Signed-off-by: Maxime CLEMENT --- .../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..30ac46cbf7a6a 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; }