From 5ec6a0e24414cc9fcf712abb45e5868069109375 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 21 Sep 2023 23:22:39 +0900 Subject: [PATCH] fix(lane_change): object filter other lane object Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 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 bf0175ccd92e4..d2aff3137ba0c 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 @@ -662,8 +662,12 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); const auto target_polygon = utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); - const auto target_backward_polygon = utils::lane_change::createPolygon( - target_backward_lanes, 0.0, std::numeric_limits::max()); + std::vector> 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::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; }