From 4305f0b602b653bce2b7b23e1431cd2d01c25efc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 5 Oct 2023 14:43:08 +0900 Subject: [PATCH] fix(lane_change): current lane obj treated as target lane obj (#5214) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 26c41006576f1..cf6f2fc01773a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -679,6 +679,16 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); std::vector> target_backward_polygons; 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::max());