From 271e68fbdfaa620dc09dd9f06bc0801a66f55094 Mon Sep 17 00:00:00 2001
From: danielsanchezaran <daniel.sanchez@tier4.jp>
Date: Wed, 13 Nov 2024 14:59:19 +0900
Subject: [PATCH] feat(start_planner, lane_departure_checker): speed up by
 updating polygons (#9309)

speed up by updating polygons

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
 .../lane_departure_checker.hpp                     |  3 ++-
 .../lane_departure_checker.cpp                     | 14 +++++++++++---
 .../src/shift_pull_out.cpp                         |  5 ++++-
 3 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp
index 9ef803d874520..6c83f8780c93c 100644
--- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp
+++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp
@@ -147,7 +147,8 @@ class LaneDepartureChecker
 
   PathWithLaneId cropPointsOutsideOfLanes(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
-    const size_t end_index);
+    const size_t end_index, std::vector<lanelet::Id> & fused_lanelets_id,
+    std::optional<autoware::universe_utils::Polygon2d> & fused_lanelets_polygon);
 
   static bool isOutOfLane(
     const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint);
diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
index a6b91c25763c5..f823988c77e4d 100644
--- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
+++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
@@ -340,13 +340,21 @@ bool LaneDepartureChecker::checkPathWillLeaveLane(
 }
 
 PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
-  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index,
+  std::vector<lanelet::Id> & fused_lanelets_id,
+  std::optional<autoware::universe_utils::Polygon2d> & fused_lanelets_polygon)
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
   PathWithLaneId temp_path;
-  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
-  if (path.points.empty() || !fused_lanelets_polygon) return temp_path;
+
+  // Update the lanelet polygon for the current path
+  if (
+    path.points.empty() || !updateFusedLaneletPolygonForPath(
+                             lanelet_map_ptr, path, fused_lanelets_id, fused_lanelets_polygon)) {
+    return temp_path;
+  }
+
   const auto vehicle_footprints =
     utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin);
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp
index 3f750a7f1a87b..a513dff2de0af 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp
@@ -66,6 +66,8 @@ std::optional<PullOutPath> ShiftPullOut::plan(
   std::vector<lanelet::Id> fused_id_start_to_end{};
   std::optional<autoware::universe_utils::Polygon2d> fused_polygon_start_to_end = std::nullopt;
 
+  std::vector<lanelet::Id> fused_id_crop_points{};
+  std::optional<autoware::universe_utils::Polygon2d> fused_polygon_crop_points = std::nullopt;
   // get safe path
   for (auto & pull_out_path : pull_out_paths) {
     universe_utils::ScopedTimeTrack st("get safe path", *time_keeper_);
@@ -120,7 +122,8 @@ std::optional<PullOutPath> ShiftPullOut::plan(
         common_parameters.ego_nearest_yaw_threshold);
 
     const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes(
-      lanelet_map_ptr, shift_path, start_segment_idx);
+      lanelet_map_ptr, shift_path, start_segment_idx, fused_id_crop_points,
+      fused_polygon_crop_points);
     if (cropped_path.points.empty()) {
       planner_debug_data.conditions_evaluation.emplace_back("cropped path is empty");
       continue;