fix(goal_planner): use departure_check_lane for path (autowarefoundat…

…ion#9423)
tier4 · Nov 22, 2024 · 2429280 · 2429280
1 parent 179c125
commit 2429280
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 14 deletions.
diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp
@@ -51,7 +51,6 @@ std::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
   if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }
-  const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes);
 
   const auto & p = parallel_parking_parameters_;
   const double max_steer_angle =
@@ -65,10 +64,12 @@ std::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
     return {};
   }
 
+  const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
+    pull_over_lanes, *planner_data_->route_handler, left_side_parking_);
   const auto arc_path = planner_.getArcPath();
 
   // check lane departure with road and shoulder lanes
-  if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {};
+  if (lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, arc_path)) return {};
 
   PullOverPath pull_over_path{};
   pull_over_path.type = getPlannerType();

diff --git a/.../behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/.../behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp
@@ -283,18 +283,12 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
         return is_footprint_in_any_polygon(footprint);
       });
   });
-  const bool is_in_lanes = std::invoke([&]() -> bool {
-    const auto drivable_lanes =
-      utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes);
-    const auto & dp = planner_data_->drivable_area_expansion_parameters;
-    const auto expanded_lanes = utils::expandLanelets(
-      drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
-      dp.drivable_area_types_to_skip);
-    const auto combined_drivable = utils::combineDrivableLanes(
-      expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes);
-    return !lane_departure_checker_.checkPathWillLeaveLane(
-      utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath());
-  });
+
+  const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
+    pull_over_lanes, *planner_data_->route_handler, left_side_parking_);
+  const bool is_in_lanes = !lane_departure_checker_.checkPathWillLeaveLane(
+    {departure_check_lane}, pull_over_path.getParkingPath());
+
   if (!is_in_parking_lots && !is_in_lanes) {
     return {};
   }