fix(behavior_path_planner): fix overlapping lane cutter (#2648)

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -74,6 +74,29 @@ void LaneFollowingModule::setParameters(const LaneFollowingParameters & paramete
   parameters_ = parameters;
 }
 
+lanelet::ConstLanelets getLaneletsFromPath(
+  const PathWithLaneId & path, const std::shared_ptr<route_handler::RouteHandler> & route_handler)
+{
+  std::vector<int64_t> unique_lanelet_ids;
+  for (const auto & p : path.points) {
+    const auto & lane_ids = p.lane_ids;
+    for (const auto & lane_id : lane_ids) {
+      if (
+        std::find(unique_lanelet_ids.begin(), unique_lanelet_ids.end(), lane_id) ==
+        unique_lanelet_ids.end()) {
+        unique_lanelet_ids.push_back(lane_id);
+      }
+    }
+  }
+
+  lanelet::ConstLanelets lanelets;
+  for (const auto & lane_id : unique_lanelet_ids) {
+    lanelets.push_back(route_handler->getLaneletsFromId(lane_id));
+  }
+
+  return lanelets;
+}
+
 PathWithLaneId LaneFollowingModule::getReferencePath() const
 {
   PathWithLaneId reference_path{};
@@ -95,16 +118,14 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
   // For current_lanes with desired length
   const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
     current_lane, current_pose, p.backward_path_length, p.forward_path_length);
-  const auto drivable_lanes = util::generateDrivableLanes(current_lanes);
 
   if (current_lanes.empty()) {
     return reference_path;
   }
 
   // calculate path with backward margin to avoid end points' instability by spline interpolation
   constexpr double extra_margin = 10.0;
-  const double backward_length =
-    std::max(p.backward_path_length, p.backward_path_length + extra_margin);
+  const double backward_length = p.backward_path_length + extra_margin;
   const auto current_lanes_with_backward_margin =
     util::calcLaneAroundPose(route_handler, current_pose, p.forward_path_length, backward_length);
   reference_path = util::getCenterLinePath(
@@ -115,6 +136,8 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
   const size_t current_seg_idx = findEgoSegmentIndex(reference_path.points);
   util::clipPathLength(
     reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length);
+  const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler);
+  const auto drivable_lanes = util::generateDrivableLanes(drivable_lanelets);
 
   {
     const int num_lane_change =

planning/behavior_path_planner/src/utilities.cpp

@@ -1123,6 +1123,10 @@ boost::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes>
     }
   }
 
+  if (overlapped_idx == lanes.size()) {
+    return {};
+  }
+
   return overlapped_idx;
 }
 
@@ -1136,30 +1140,30 @@ std::vector<DrivableLanes> cutOverlappedLanes(
 
   const std::vector<DrivableLanes> shorten_lanes{
     lanes.begin(), lanes.begin() + *overlapped_lanelet_id};
-  const std::vector<DrivableLanes> removed_lanes{
-    lanes.begin() + *overlapped_lanelet_id, lanes.end()};
-
-  const auto transformed_lanes = util::transformToLanelets(removed_lanes);
-
-  auto isIncluded = [&transformed_lanes](const std::vector<int64_t> & lane_ids) {
-    if (transformed_lanes.empty() || lane_ids.empty()) return false;
-
-    for (const auto & transformed_lane : transformed_lanes) {
-      for (const auto & lane_id : lane_ids) {
-        if (lane_id == transformed_lane.id()) {
-          return true;
-        }
+  const auto shorten_lanelets = util::transformToLanelets(shorten_lanes);
+
+  // create removed lanelets
+  std::vector<int64_t> removed_lane_ids;
+  for (size_t i = *overlapped_lanelet_id; i < lanes.size(); ++i) {
+    const auto target_lanelets = util::transformToLanelets(lanes.at(i));
+    for (const auto & target_lanelet : target_lanelets) {
+      // if target lane is inside of the shorten lanelets, we do not remove it
+      if (checkHasSameLane(shorten_lanelets, target_lanelet)) {
+        continue;
       }
+      removed_lane_ids.push_back(target_lanelet.id());
     }
-
-    return false;
-  };
+  }
 
   for (size_t i = 0; i < path.points.size(); ++i) {
     const auto & lane_ids = path.points.at(i).lane_ids;
-    if (isIncluded(lane_ids)) {
-      path.points.erase(path.points.begin() + i, path.points.end());
-      break;
+    for (const auto & lane_id : lane_ids) {
+      if (
+        std::find(removed_lane_ids.begin(), removed_lane_ids.end(), lane_id) !=
+        removed_lane_ids.end()) {
+        path.points.erase(path.points.begin() + i, path.points.end());
+        break;
+      }
     }
   }