fix(goal_planner): use departure_check_lane for path (#9423)

autowarefoundation · Nov 22, 2024 · 328afb4 · 328afb4
1 parent 834ba99
commit 328afb4
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 14 deletions.
diff --git a/.../autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/.../autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp
@@ -55,7 +55,6 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   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 =
@@ -69,10 +68,12 @@ std::optional<PullOverPath> GeometricPullOver::plan(
     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 {};
 
   auto pull_over_path_opt = PullOverPath::create(
     getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose,

diff --git a/...nner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/...nner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp
@@ -292,18 +292,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.parking_path());
-  });
+
+  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.parking_path());
+
   if (!is_in_parking_lots && !is_in_lanes) {
     return {};
   }