feat(start_planner, lane_departure_checker): speed up by updating pol…

…ygons (autowarefoundation#9309) speed up by updating polygons Signed-off-by: Daniel Sanchez <[email protected]>
tier4 · Nov 13, 2024 · 271e68f · 271e68f
1 parent 7b2e1b6
commit 271e68f
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 5 deletions.
diff --git a/...lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/...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/...utoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/...utoware_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/.../behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/.../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;