From 91568cc9eab66ac4d2750835ee723f5802c3a47b Mon Sep 17 00:00:00 2001
From: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Date: Wed, 18 Jan 2023 14:20:18 +0900
Subject: [PATCH] fix(behavior_path_planner): improve isPathInLanelet function
 for lane change

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../scene_module/lane_change/util.hpp         |  5 +++
 .../src/scene_module/lane_change/util.cpp     | 37 +++++++++++++++++--
 2 files changed, 39 insertions(+), 3 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
index d91a9373bb492..cf6e88edf2c16 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
@@ -48,6 +48,11 @@ using tier4_autoware_utils::Polygon2d;
 
 PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
 
+bool isPathInLanelets(
+  const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
+  const double original_lane_length, const lanelet::ConstLanelets & target_lanelets,
+  const double target_lane_length);
+
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets);
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
index fc0305e9b402b..09d0bc3868b64 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
@@ -59,6 +59,31 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith
   return path;
 }
 
+bool isPathInLanelets(
+  const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
+  const double original_lane_length, const lanelet::ConstLanelets & target_lanelets,
+  const double target_lane_length)
+{
+  const auto current_lane_poly =
+    lanelet::utils::getPolygonFromArcLength(original_lanelets, 0, original_lane_length);
+  const auto target_lane_poly =
+    lanelet::utils::getPolygonFromArcLength(target_lanelets, 0, target_lane_length);
+  const auto current_lane_poly_2d = lanelet::utils::to2D(current_lane_poly).basicPolygon();
+  const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_poly).basicPolygon();
+  for (const auto & pt : path.points) {
+    const lanelet::BasicPoint2d ll_pt(pt.point.pose.position.x, pt.point.pose.position.y);
+    const auto is_in_current = boost::geometry::covered_by(ll_pt, current_lane_poly_2d);
+    if (is_in_current){
+      continue;
+    }
+    const auto is_in_target = boost::geometry::covered_by(ll_pt, target_lane_poly_2d);
+    if (!is_in_target) {
+      return false;
+    }
+  }
+  return true;
+}
+
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets)
@@ -98,7 +123,8 @@ double getDistanceWhenDecelerate(
 std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
-  const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
+  const lanelet::ConstLanelets & original_lanelets, const double original_lane_length,
+  const lanelet::ConstLanelets & target_lanelets, const double target_lane_length,
   const double acceleration, const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed,
   const LaneChangeParameters & lane_change_param)
 {
@@ -175,7 +201,9 @@ std::optional<LaneChangePath> constructCandidatePath(
   }
 
   // check candidate path is in lanelet
-  if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) {
+  if (!isPathInLanelets(
+        candidate_path.path, original_lanelets, original_lane_length, target_lanelets,
+        target_lane_length)) {
     return std::nullopt;
   }
 
@@ -228,6 +256,8 @@ LaneChangePaths getLaneChangePaths(
 
   const auto arc_position_from_current = lanelet::utils::getArcCoordinates(original_lanelets, pose);
   const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose);
+
+  const auto original_lane_length = lanelet::utils::getLaneletLength2d(original_lanelets);
   const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets);
 
   for (double acceleration = 0.0; acceleration >= -maximum_deceleration;
@@ -290,7 +320,8 @@ LaneChangePaths getLaneChangePaths(
     const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed};
     const auto candidate_path = constructCandidatePath(
       prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path,
-      shift_line, original_lanelets, target_lanelets, acceleration, lc_dist, lc_speed, parameter);
+      shift_line, original_lanelets, original_lane_length, target_lanelets, target_lane_length,
+      acceleration, lc_dist, lc_speed, parameter);
 
     if (!candidate_path) {
       continue;