feat(lane_change): update lane change object filter (#4172)

* feat(lane_change): update lane change object filter Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> --------- Signed-off-by: yutaka <purewater0901@gmail.com>
autowarefoundation · Jul 5, 2023 · 58b2044 · 58b2044
1 parent 3d2f446
commit 58b2044
Show file tree

Hide file tree

Showing 2 changed files with 11 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
@@ -777,7 +777,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
 
   // get lanes used for detection
   const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets(
-    route_handler, path.target_lanelets, current_pose, lane_change_parameters.backward_lane_length);
+    route_handler, target_lanes, current_pose, lane_change_parameters.backward_lane_length);
 
   const auto dynamic_object_indices = utils::lane_change::filterObject(
     *dynamic_objects, current_lanes, target_lanes, backward_target_lanes_for_object_filtering,

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -1156,11 +1156,20 @@ LaneChangeTargetObjectIndices filterObject(
     }
 
     // check if the object intersects with current lanes
-    if (current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon)) {
+    if (
+      current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) &&
+      max_dist_ego_to_obj >= 0.0) {
+      // check only the objects that are in front of the ego vehicle
       filtered_obj_indices.current_lane.push_back(i);
       continue;
     }
 
+    // ignore all objects that are behind the ego vehicle and not on the current and target
+    // lanes
+    if (max_dist_ego_to_obj < 0.0) {
+      continue;
+    }
+
     filtered_obj_indices.other_lane.push_back(i);
   }