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 41561493e08d6..361cb5548dc40 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 @@ -777,7 +777,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const // get lanes used for detection const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( - route_handler, path.target_lanelets, current_pose, lane_change_parameters.backward_lane_length); + route_handler, target_lanes, current_pose, lane_change_parameters.backward_lane_length); const auto dynamic_object_indices = utils::lane_change::filterObject( *dynamic_objects, current_lanes, target_lanes, backward_target_lanes_for_object_filtering, diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 6d1b4486856e1..44b4275bfa2f0 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1156,11 +1156,20 @@ LaneChangeTargetObjectIndices filterObject( } // check if the object intersects with current lanes - if (current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon)) { + if ( + current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && + max_dist_ego_to_obj >= 0.0) { + // check only the objects that are in front of the ego vehicle filtered_obj_indices.current_lane.push_back(i); continue; } + // ignore all objects that are behind the ego vehicle and not on the current and target + // lanes + if (max_dist_ego_to_obj < 0.0) { + continue; + } + filtered_obj_indices.other_lane.push_back(i); }