From 0a881f35fc55714934294762e4bdd37a8f97e0d9 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 13 Nov 2024 15:56:01 +0900 Subject: [PATCH] feat(start_planner):speed up start planner v0.36 (#1639) feat(start_planner, lane_departure_checker): speed up by updating polygons (#9309) speed up by updating polygons Signed-off-by: Daniel Sanchez --- .../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 & fused_lanelets_id, + std::optional & 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 & fused_lanelets_id, + std::optional & 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 ShiftPullOut::plan( std::vector fused_id_start_to_end{}; std::optional fused_polygon_start_to_end = std::nullopt; + std::vector fused_id_crop_points{}; + std::optional 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 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;