From 738c3763c28c16f537c0f0305f9cc66531b710ea Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Oct 2023 09:57:42 +0900 Subject: [PATCH] fix(lane_change): filter out objects out of lane to fix stopping margin chattering (#5321) * fix(lane_change): filter out objects out of lane to fix stopping margin chattering Signed-off-by: kosuke55 * use boost intersects Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../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 769b0d681d2ac..3a2903cb01e9e 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 @@ -282,6 +282,16 @@ void NormalLaneChange::insertStopPoint( if (v > lane_change_parameters_->stop_velocity_threshold) { return false; } + + // target_objects includes objects out of target lanes, so filter them out + if (!boost::geometry::intersects( + tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + lanelet::utils::combineLaneletsShape(status_.target_lanes) + .polygon2d() + .basicPolygon())) { + return false; + } + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); return stopping_distance_for_obj < distance_to_target_lane_obj && distance_to_target_lane_obj < distance_to_ego_lane_obj;