From a87d526aa24e32e20a5ebb72ed6c4c180d40f7d3 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Fri, 8 Sep 2023 17:10:04 +0900 Subject: [PATCH] fix(lane_change): fix target object filter Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) 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 a81f6227e4064..7f7d4f2a6cb4e 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 @@ -694,6 +694,12 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } + const auto is_lateral_far = [&]() { + 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; + }; + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -704,8 +710,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target // lanes - filtered_obj_indices.target_lane.push_back(i); - continue; + if (is_lateral_far()) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } } // check if the object intersects with target backward lanes