fix(start_planner): crop backward path to start from narrow space (au…

…towarefoundation#4929) * fix(start_planner): crop backward path to start from narrow space Signed-off-by: kosuke55 <[email protected]> * check out of lanes Signed-off-by: kosuke55 <[email protected]> * refactor Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
tier4 · Sep 16, 2023 · 726fd7f · 726fd7f
1 parent 5614761
commit 726fd7f
Showing 1 changed file with 37 additions and 6 deletions.
diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
@@ -96,17 +96,48 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P
         shift_path.points.begin() + collision_check_end_idx + 1);
     }
 
-    // check lane departure
+    // extract shoulder lanes from pull out lanes
+    lanelet::ConstLanelets shoulder_lanes;
+    std::copy_if(
+      pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes),
+      [&route_handler](const auto & pull_out_lane) {
+        return route_handler->isShoulderLanelet(pull_out_lane);
+      });
     const auto drivable_lanes =
-      utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_out_lanes);
+      utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
     const auto & dp = planner_data_->drivable_area_expansion_parameters;
-    const auto expanded_lanes = utils::expandLanelets(
+    const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets(
       drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
-      dp.drivable_area_types_to_skip);
+      dp.drivable_area_types_to_skip));
+
+    // crop backward path
+    // removes points which are out of lanes up to the start pose.
+    // this ensures that the backward_path stays within the drivable area when starting from a
+    // narrow place.
+    const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+      shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold,
+      common_parameters.ego_nearest_yaw_threshold);
+    PathWithLaneId cropped_path{};
+    for (size_t i = 0; i < shift_path.points.size(); ++i) {
+      const Pose pose = shift_path.points.at(i).point.pose;
+      const auto transformed_vehicle_footprint =
+        transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose));
+      const bool is_out_of_lane =
+        LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint);
+      if (i <= start_segment_idx) {
+        if (!is_out_of_lane) {
+          cropped_path.points.push_back(shift_path.points.at(i));
+        }
+      } else {
+        cropped_path.points.push_back(shift_path.points.at(i));
+      }
+    }
+    shift_path.points = cropped_path.points;
+
+    // check lane departure
     if (
       parameters_.check_shift_path_lane_departure &&
-      lane_departure_checker_->checkPathWillLeaveLane(
-        utils::transformToLanelets(expanded_lanes), path_start_to_end)) {
+      lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) {
       continue;
     }