fix(lane_change): object filter other lane object

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

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -662,8 +662,12 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
   const auto target_polygon =
     utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
-  const auto target_backward_polygon = utils::lane_change::createPolygon(
-    target_backward_lanes, 0.0, std::numeric_limits<double>::max());
+  std::vector<std::optional<lanelet::BasicPolygon2d>> target_backward_polygons;
+  for (const auto & target_backward_lane : target_backward_lanes) {
+    lanelet::ConstLanelets lanelet{target_backward_lane};
+    auto lane_polygon =
+      utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits<double>::max());
+  }
 
   // minimum distance to lane changing start point
   const double t_prepare = common_parameters.lane_change_prepare_duration;
@@ -716,10 +720,22 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
       continue;
     }
 
+    const auto check_backward_polygon = [&](const auto & target_backward_polygon) {
+      if (
+        target_backward_polygon &&
+        boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) {
+        const auto lateral = tier4_autoware_utils::calcLateralDeviation(
+          current_pose, object.kinematics.initial_pose_with_covariance.pose.position);
+        return (std::abs(lateral) > common_parameters.vehicle_width);
+      }
+      return false;
+    };
+
     // check if the object intersects with target backward lanes
     if (
-      target_backward_polygon &&
-      boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) {
+      !target_backward_polygons.empty() &&
+      std::any_of(
+        target_backward_polygons.begin(), target_backward_polygons.end(), check_backward_polygon)) {
       filtered_obj_indices.target_lane.push_back(i);
       continue;
     }