feat(lane_change): revise current lane objects filtering

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -886,6 +886,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `backward_length_buffer_for_end_of_lane`     | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change                                  | 3.0                |
 | `backward_length_buffer_for_blocking_object` | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0                |
 | `backward_length_from_intersection`          | [m]    | double | Distance threshold from the last intersection to invalidate or cancel the lane change path                             | 5.0                |
+| `enable_stopped_vehicle_buffer`              | [-]    | bool   | If true, will keep enough distance from current lane front stopped object to perform lane change when possible         | true               |
 | `trajectory.max_prepare_duration`            | [s]    | double | The maximum preparation time for the ego vehicle to be ready to perform lane change.                                   | 4.0                |
 | `trajectory.min_prepare_duration`            | [s]    | double | The minimum preparation time for the ego vehicle to be ready to perform lane change.                                   | 2.0                |
 | `trajectory.lateral_jerk`                    | [m/s3] | double | Lateral jerk value for lane change path generation                                                                     | 0.5                |

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -5,6 +5,7 @@
       backward_length_buffer_for_end_of_lane: 3.0 # [m]
       backward_length_buffer_for_blocking_object: 3.0 # [m]
       backward_length_from_intersection: 5.0 # [m]
+      enable_stopped_vehicle_buffer: true
 
       # turn signal
       min_length_for_turn_signal_activation: 10.0 # [m]

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp

@@ -149,6 +149,7 @@ struct Parameters
   double backward_length_buffer_for_end_of_lane{0.0};
   double backward_length_buffer_for_blocking_object{0.0};
   double backward_length_from_intersection{5.0};
+  bool enable_stopped_vehicle_buffer{false};
 
   // parked vehicle
   double object_check_min_road_shoulder_width{0.5};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -195,6 +195,8 @@
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_blocking_object"));
   p.backward_length_from_intersection =
     getOrDeclareParameter<double>(*node, parameter("backward_length_from_intersection"));
+  p.enable_stopped_vehicle_buffer =
+    getOrDeclareParameter<bool>(*node, parameter("enable_stopped_vehicle_buffer"));
 
   if (p.backward_length_buffer_for_end_of_lane < 1.0) {
     RCLCPP_WARN_STREAM(
@@ -305,6 +307,8 @@
       p->backward_length_buffer_for_blocking_object);
     updateParam<double>(
       parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer);
+    updateParam<bool>(
+      parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer);
 
     updateParam<double>(
       parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -515,7 +515,9 @@
   const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width);
 
   const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path";
-  if (filtered_objects_.current_lane.empty()) {
+  if (
+    filtered_objects_.current_lane.empty() ||
+    !lane_change_parameters_->enable_stopped_vehicle_buffer) {
     set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason);
     return;
   }
@@ -947,33 +949,27 @@
   target_lane_leading.stopped_at_bound.reserve(reserve_size);
   filtered_objects.target_lane_trailing.reserve(reserve_size);
   filtered_objects.others.reserve(reserve_size);
 
-  const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity;
-
   for (const auto & object : objects.objects) {
     auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_);
     const auto & ext_obj_pose = ext_object.initial_pose;
     ext_object.dist_from_ego = autoware::motion_utils::calcSignedArcLength(
       current_lanes_ref_path.points, current_pose.position, ext_obj_pose.position);
 
     const auto is_before_terminal =
       utils::lane_change::is_before_terminal(common_data_ptr_, current_lanes_ref_path, ext_object);
 
     const auto ahead_of_ego =
       utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, ext_object);
 
     if (utils::lane_change::filter_target_lane_objects(
           common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego,
           is_before_terminal, target_lane_leading, filtered_objects.target_lane_trailing)) {
       continue;
     }
 
-    // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior
-    const auto is_moving = velocity_filter(
-      ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits<double>::max());
-
     if (
-      ahead_of_ego && is_moving && is_before_terminal &&
+      ahead_of_ego && is_before_terminal &&
       !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) {
       // check only the objects that are in front of the ego vehicle
       filtered_objects.current_lane.push_back(ext_object);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -920,19 +920,17 @@
       continue;
     }
 
-    // calculate distance from path front to the stationary object polygon on the ego lane.
-    for (const auto & polygon_p : object.initial_polygon.outer()) {
-      const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d());
-      const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp);
-
-      // ignore if the point is not on ego path
-      if (std::abs(lateral_fp) > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) {
-        continue;
-      }
-
-      const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp);
-      min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj);
+    // check if object is on ego path
+    const auto obj_half_width = object.shape.dimensions.y / 2;
+    const auto obj_lat_dist_to_path =
+      std::abs(motion_utils::calcLateralOffset(path_points, object.initial_pose.position)) -
+      obj_half_width;
+    if (obj_lat_dist_to_path > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) {
+      continue;
     }
+
+    min_dist_to_obj = std::min(min_dist_to_obj, dist_to_obj);
+    break;
   }
   return min_dist_to_obj;
 }

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp

@@ -242,15 +242,14 @@ TEST_F(TestNormalLaneChange, testFilteredObjects)
 
   const auto & filtered_objects = get_filtered_objects();
 
-  // Note: There's 1 stopping object in current lanes, however, it was filtered out.
   const auto filtered_size =
     filtered_objects.current_lane.size() + filtered_objects.target_lane_leading.size() +
     filtered_objects.target_lane_trailing.size() + filtered_objects.others.size();
   EXPECT_EQ(filtered_size, planner_data_->dynamic_object->objects.size());
-  EXPECT_EQ(filtered_objects.current_lane.size(), 0);
+  EXPECT_EQ(filtered_objects.current_lane.size(), 1);
   EXPECT_EQ(filtered_objects.target_lane_leading.size(), 2);
   EXPECT_EQ(filtered_objects.target_lane_trailing.size(), 0);
-  EXPECT_EQ(filtered_objects.others.size(), 2);
+  EXPECT_EQ(filtered_objects.others.size(), 1);
 }
 
 TEST_F(TestNormalLaneChange, testGetPathWhenValid)