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 <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Maxime CLEMENT <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Oct 8, 2024 · 19c7cb7 · 19c7cb7
1 parent 6215c64
commit 19c7cb7
Showing 1 changed file with 10 additions and 0 deletions.
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
@@ -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;
 }