From df6bfb459837f848928dc832c0203b4aee82aef8 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 22 Aug 2023 23:19:52 +0900 Subject: [PATCH] fix(lane_change): output lane change path even if the start point is in target lane (#4707) Signed-off-by: satoshi-ota --- .../src/scene_module/lane_change/normal.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) 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 a8fdfce54b145..dad925c6b2335 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 @@ -974,7 +974,16 @@ bool NormalLaneChange::getLaneChangePaths( const lanelet::BasicPoint2d lc_start_point( prepare_segment.points.back().point.pose.position.x, prepare_segment.points.back().point.pose.position.y); - if (!boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d)) { + + const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( + target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + + if (!is_valid_start_point) { // lane changing points are not inside of the target preferred lanes or its neighbors continue; }