feat(lane_change): revise current lane objects filtering (autowarefoundation#9785) (#1769)

* consider stopped front objects



* simplify computation of dist to front current lane object



* add flag to enable/disable keeping distance from front stopped vehicle



* fix object filtering test



---------

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

Author: kyoichi-sugahara

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

@@ -194,6 +194,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
     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(
@@ -304,6 +306,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
       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

@@ -552,7 +552,9 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path)
 
   const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width);
 
-  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);
     return;
   }
@@ -1022,8 +1024,6 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const
   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;
@@ -1042,12 +1042,8 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const
       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

@@ -1101,19 +1101,17 @@ double get_min_dist_to_current_lanes_obj(
       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

@@ -209,6 +209,28 @@ TEST_F(TestNormalLaneChange, testGetPathWhenInvalid)
   ASSERT_FALSE(lc_status.is_valid_path);
 }
 
+TEST_F(TestNormalLaneChange, testFilteredObjects)
+{
+  constexpr auto is_approved = true;
+  ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0);
+  planner_data_->self_odometry = set_odometry(ego_pose_);
+  set_previous_approved_path();
+
+  normal_lane_change_->update_lanes(!is_approved);
+  normal_lane_change_->update_filtered_objects();
+
+  const auto & filtered_objects = get_filtered_objects();
+
+  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(), 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(), 1);
+}
+
 TEST_F(TestNormalLaneChange, testGetPathWhenValid)
 {
   constexpr auto is_approved = true;