fix(lane_change): fix target object filter (autowarefoundation#4930)

* fix(lane_change): fix target object filter Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * used lateral distance from centerline instead Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> Signed-off-by: Zulfaqar Azmi <[email protected]>
tier4 · Sep 15, 2023 · 50ecbd5 · 50ecbd5
1 parent 71f1218
commit 50ecbd5
Showing 1 changed file with 14 additions and 2 deletions.
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
@@ -665,6 +665,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
   const auto target_backward_polygon = utils::lane_change::createPolygon(
     target_backward_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto dist_ego_to_current_lanes_center =
+    lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
 
   LaneChangeTargetObjectIndices filtered_obj_indices;
   for (size_t i = 0; i < objects.objects.size(); ++i) {
@@ -691,6 +693,14 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
       min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj);
     }
 
+    const auto is_lateral_far = [&]() {
+      const auto dist_object_to_current_lanes_center =
+        lanelet::utils::getLateralDistanceToClosestLanelet(
+          current_lanes, object.kinematics.initial_pose_with_covariance.pose);
+      const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center;
+      return std::abs(lateral) > (common_parameters.vehicle_width / 2);
+    };
+
     // ignore static object that are behind the ego vehicle
     if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) {
       continue;
@@ -701,8 +711,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
       // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target
       // lanes
 
-      filtered_obj_indices.target_lane.push_back(i);
-      continue;
+      if (max_dist_ego_to_obj >= 0 || is_lateral_far()) {
+        filtered_obj_indices.target_lane.push_back(i);
+        continue;
+      }
     }
 
     // check if the object intersects with target backward lanes