Use preceeding lanes

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -296,7 +296,7 @@ double calcPhaseLength(
 
 LanesPolygon createLanesPolygon(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
-  const lanelet::ConstLanelets & target_backward_lanes);
+  const std::vector<lanelet::ConstLanelets> & target_backward_lanes);
 }  // namespace behavior_path_planner::utils::lane_change
 
 namespace behavior_path_planner::utils::lane_change::debug

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1022,14 +1022,45 @@ void NormalLaneChange::filterObjectsByLanelets(
 
   // get backward lanes
   const auto backward_length = lane_change_parameters_->backward_lane_length;
-  const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets(
-    *route_handler, target_lanes, current_pose, backward_length);
+  const auto target_backward_lanes =
+    [&]() -> std::vector<lanelet::ConstLanelets> {
+      if (target_lanes.empty()) {
+        return {};
+      }
+
+      const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);
+
+      if (arc_length.length >= backward_length) {
+        return {};
+      }
+
+      const auto & front_lane = target_lanes.front();
+      return route_handler->getPrecedingLaneletSequence(
+        front_lane, std::abs(backward_length - arc_length.length), {front_lane});
+    }();
 
   {
     lane_change_debug_.current_lanes = current_lanes;
     lane_change_debug_.target_lanes = target_lanes;
-    lane_change_debug_.target_backward_lanes = target_backward_lanes;
+
+    lanelet::ConstLanelets backward_lanes{};
+    const auto num_of_lanes = std::invoke([&target_backward_lanes]() {
+      size_t sum{0};
+      for (const auto & lanes : target_backward_lanes) {
+        sum += lanes.size();
+      }
+      return sum;
+    });
+
+    backward_lanes.reserve(num_of_lanes);
+
+    for (const auto & lanes : target_backward_lanes) {
+      backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end());
   }
+
+    lane_change_debug_.target_backward_lanes = backward_lanes;
+  }
+
   const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(
     target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset,
     lane_change_parameters_->lane_expansion_right_offset);

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -1227,7 +1227,7 @@ double calcPhaseLength(
 
 LanesPolygon createLanesPolygon(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
-  const lanelet::ConstLanelets & target_backward_lanes)
+  const std::vector<lanelet::ConstLanelets> & target_backward_lanes)
 {
   LanesPolygon lanes_polygon;
 
@@ -1237,19 +1237,8 @@ LanesPolygon createLanesPolygon(
     utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
 
   for (const auto & target_backward_lane : target_backward_lanes) {
-    // Check to see is target_backward_lane is in current_lanes
-    // Without this check, current lane object might be treated as target lane object
-    const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) {
-      return current_lane.id() == target_backward_lane.id();
-    };
-
-    if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) {
-      continue;
-    }
-
-    lanelet::ConstLanelets lanelet{target_backward_lane};
     auto lane_polygon =
-      utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits<double>::max());
+      utils::lane_change::createPolygon(target_backward_lane, 0.0, std::numeric_limits<double>::max());
 
     if (lane_polygon) {
       lanes_polygon.target_backward.push_back(*lane_polygon);