lc - cherry pick filter 3rd lane moving object

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1195,7 +1195,7 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets(
     };
 
     if (
-      check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far &&
+      check_optional_polygon(object, lanes_polygon.target) && is_lateral_far &&
       is_before_terminal()) {
       const auto ahead_of_ego =
         utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object);
@@ -1207,6 +1207,19 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets(
       continue;
     }
 
+    if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_before_terminal()) {
+      const auto ahead_of_ego =
+        utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object);
+      if (object.kinematics.initial_twist_with_covariance.twist.linear.x < 1.0) {
+        if (ahead_of_ego) {
+          target_lane_leading_objects.push_back(object);
+        } else {
+          target_lane_trailing_objects.push_back(object);
+        }
+        continue;
+      }
+    }
+
     const auto is_overlap_target_backward = std::invoke([&]() -> bool {
       const auto check_backward_polygon = [&object](const auto & target_backward_polygon) {
         return isPolygonOverlapLanelet(object, target_backward_polygon);

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -38,6 +38,7 @@
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 
 #include <boost/geometry/algorithms/detail/disjoint/interface.hpp>
+#include <boost/geometry/algorithms/difference.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/LineString.h>
@@ -1212,11 +1213,18 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr)
   const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(
     lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset,
     lc_param_ptr->lane_expansion_right_offset);
-  lanes_polygon.expanded_target = utils::lane_change::createPolygon(
+
+  const auto expanded_target_polygon = utils::lane_change::createPolygon(
     expanded_target_lanes, 0.0, std::numeric_limits<double>::max());
 
   lanes_polygon.target_neighbor = *utils::lane_change::createPolygon(
     lanes->target_neighbor, 0.0, std::numeric_limits<double>::max());
+  std::vector<lanelet::BasicPolygon2d> difference;
+  boost::geometry::difference(*expanded_target_polygon, *lanes_polygon.target, difference);
+
+  if (!difference.empty()) {
+    lanes_polygon.expanded_target = std::make_optional<lanelet::BasicPolygon2d>(difference.front());
+  }
 
   lanes_polygon.preceding_target.reserve(lanes->preceding_target.size());
   for (const auto & preceding_lane : lanes->preceding_target) {