From 776c1180adcc75ac614fde2d79b69d8f273159ea Mon Sep 17 00:00:00 2001
From: Fumiya Watanabe <rej55.g@gmail.com>
Date: Thu, 26 Oct 2023 19:07:34 +0900
Subject: [PATCH] fix(lane_change): filter objects for skip parking objects

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
---
 .../scene_module/lane_change/normal.hpp       |  3 ++
 .../src/scene_module/lane_change/normal.cpp   | 28 ++++++++++++++++---
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
index 54883489f2fe6..dc9f44fbd53da 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -155,6 +155,9 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
+  std::vector<ExtendedPredictedObject> filterObjectsInTargetLane(
+    const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const;
+
   //! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
   //! @param obstacle_check_distance Distance to check ahead for any objects that might be
   //! obstructing ego path. It makes sense to use values like the maximum lane change distance.
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 d3a120b43a073..309f2ee3e0b97 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
@@ -914,6 +914,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   return filtered_obj_indices;
 }
 
+std::vector<ExtendedPredictedObject> NormalLaneChange::filterObjectsInTargetLane(
+  const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const
+{
+  const auto target_polygon =
+    utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
+  std::vector<ExtendedPredictedObject> filtered_objects{};
+  if (target_polygon) {
+    for (auto & obj : objects.target_lane) {
+      const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape);
+      if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
+        filtered_objects.push_back(obj);
+      }
+    }
+  }
+
+  return filtered_objects;
+}
+
 PathWithLaneId NormalLaneChange::getTargetSegment(
   const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose,
   const double target_lane_length, const double lane_changing_length,
@@ -1293,11 +1311,13 @@ bool NormalLaneChange::getLaneChangePaths(
         }
 
         candidate_paths->push_back(*candidate_path);
+
+        std::vector<ExtendedPredictedObject> filtered_objects =
+          filterObjectsInTargetLane(target_objects, target_lanes);
         if (
-          !is_stuck &&
-          utils::lane_change::passParkedObject(
-            route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
-            is_goal_in_route, *lane_change_parameters_, object_debug_)) {
+          !is_stuck && utils::lane_change::passParkedObject(
+                         route_handler, *candidate_path, filtered_objects, lane_change_buffer,
+                         is_goal_in_route, *lane_change_parameters_, object_debug_)) {
           debug_print(
             "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip "
             "lane change.");