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 <maxime.clement@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: maxime-clem

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