fix(behavior_path_planner): fix lane change logic on the edge case (a…

…utowarefoundation#2287)

* fix(behavior_path_planner): fix lane change logic on the edge case

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

* fix pose init and add failed reasons

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

* Ignore prepare distance if object is static

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

* parameterize the target speed threshold to ignore in prepare phase

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

* Set default value for lane change parameters

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

* additional parameterized

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

* update readme and default value for prepare phase collision check

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

* change force candidatePath() color

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

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

planning/behavior_path_planner/behavior_path_planner_lane_change.md

@@ -301,6 +301,14 @@ Finally minimum longitudinal distance for `d_rear` is added to compensate for ob
 std::max(longitudinal_distance_min_threshold, d_rear) < d_front + d_inter
 ```
 
+##### Collision check in prepare phase
+
+The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases.
+
+![enable collision check at prepare phase](./image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png)
+
+The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects.
+
 #### If the lane is blocked and multiple lane changes
 
 When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance computation is as follows.
@@ -370,18 +378,19 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`.
 
 The following parameters are configurable in `lane_change.param.yaml`.
 
-| Name                              | Unit    | Type    | Description                                                                             | Default value |
-| :-------------------------------- | ------- | ------- | --------------------------------------------------------------------------------------- | ------------- |
-| `lane_change_prepare_duration`    | [m]     | double  | The preparation time for the ego vehicle to be ready to perform lane change.            | 4.0           |
-| `lane_changing_duration`          | [m]     | double  | The total time that is taken to complete the lane-changing task.                        | 8.0           |
-| `lane_change_finish_judge_buffer` | [m]     | double  | The additional buffer used to confirm lane change process completion                    | 3.0           |
-| `minimum_lane_change_velocity`    | [m/s]   | double  | Minimum speed during lane changing process.                                             | 5.6           |
-| `prediction_time_resolution`      | [s]     | double  | Time resolution for object's path interpolation and collision check.                    | 0.5           |
-| `maximum_deceleration`            | [m/s^2] | double  | Ego vehicle maximum deceleration when performing lane change.                           | 1.0           |
-| `lane_change_sampling_num`        | [-]     | int     | Number of possible lane-changing trajectories that are being influenced by deceleration | 10            |
-| `enable_abort_lane_change`        | [-]     | boolean | Enable abort lane change. (\*1)                                                         | false         |
-| `use_all_predicted_path`          | [-]     | boolean | If false, use only the predicted path that has the maximum confidence.                  | false         |
-| `lane_change_search_distance`     | [m]     | double  | The turn signal activation distance during the lane change preparation phase.           | 30.0          |
+| Name                                       | Unit    | Type    | Description                                                                                                                                                | Default value |
+| :----------------------------------------- | ------- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- |
+| `lane_change_prepare_duration`             | [m]     | double  | The preparation time for the ego vehicle to be ready to perform lane change.                                                                               | 4.0           |
+| `lane_changing_duration`                   | [m]     | double  | The total time that is taken to complete the lane-changing task.                                                                                           | 8.0           |
+| `lane_change_finish_judge_buffer`          | [m]     | double  | The additional buffer used to confirm lane change process completion                                                                                       | 3.0           |
+| `minimum_lane_change_velocity`             | [m/s]   | double  | Minimum speed during lane changing process.                                                                                                                | 5.6           |
+| `prediction_time_resolution`               | [s]     | double  | Time resolution for object's path interpolation and collision check.                                                                                       | 0.5           |
+| `maximum_deceleration`                     | [m/s^2] | double  | Ego vehicle maximum deceleration when performing lane change.                                                                                              | 1.0           |
+| `lane_change_sampling_num`                 | [-]     | int     | Number of possible lane-changing trajectories that are being influenced by deceleration                                                                    | 10            |
+| `use_all_predicted_path`                   | [-]     | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                     | false         |
+| `lane_change_search_distance`              | [m]     | double  | The turn signal activation distance during the lane change preparation phase.                                                                              | 30.0          |
+| `enable_collision_check_at_prepare_phase`  | [-]     | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase.                                   | false         |
+| `prepare_phase_ignore_target_speed_thresh` | [m/s]   | double  | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1           |
 
 ### Collision checks during lane change
 

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -27,9 +27,9 @@
 
     visualize_drivable_area_for_shared_linestrings_lanelet: true
 
-    lateral_distance_max_threshold: 5.0
+    lateral_distance_max_threshold: 2.0
     longitudinal_distance_min_threshold: 3.0
-    expected_front_deceleration: -1.0
+    expected_front_deceleration: -0.5
     expected_rear_deceleration: -1.0
     rear_vehicle_reaction_time: 2.0
     rear_vehicle_safety_time_margin: 2.0

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

@@ -12,8 +12,9 @@
       abort_lane_change_velocity_thresh: 0.5
       abort_lane_change_angle_thresh: 10.0      # [deg]
       abort_lane_change_distance_thresh: 0.3    # [m]
+      prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s]
       enable_abort_lane_change: true
-      enable_collision_check_at_prepare_phase: true
+      enable_collision_check_at_prepare_phase: false
       use_predicted_path_outside_lanelet: true
       use_all_predicted_path: true
       publish_debug_marker: false

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp

@@ -25,25 +25,26 @@ namespace behavior_path_planner
 {
 struct LaneChangeParameters
 {
-  double lane_change_prepare_duration;
-  double lane_changing_duration;
-  double minimum_lane_change_prepare_distance;
-  double lane_change_finish_judge_buffer;
-  double minimum_lane_change_velocity;
-  double prediction_time_resolution;
-  double maximum_deceleration;
-  int lane_change_sampling_num;
-  double abort_lane_change_velocity_thresh;
-  double abort_lane_change_angle_thresh;
-  double abort_lane_change_distance_thresh;
-  bool enable_abort_lane_change;
-  bool enable_collision_check_at_prepare_phase;
-  bool use_predicted_path_outside_lanelet;
-  bool use_all_predicted_path;
-  bool publish_debug_marker;
+  double lane_change_prepare_duration{2.0};
+  double lane_changing_duration{4.0};
+  double minimum_lane_change_prepare_distance{4.0};
+  double lane_change_finish_judge_buffer{3.0};
+  double minimum_lane_change_velocity{5.6};
+  double prediction_time_resolution{0.5};
+  double maximum_deceleration{1.0};
+  int lane_change_sampling_num{10};
+  double abort_lane_change_velocity_thresh{0.5};
+  double abort_lane_change_angle_thresh{0.174533};
+  double abort_lane_change_distance_thresh{0.3};
+  double prepare_phase_ignore_target_speed_thresh{0.1};
+  bool enable_abort_lane_change{true};
+  bool enable_collision_check_at_prepare_phase{true};
+  bool use_predicted_path_outside_lanelet{false};
+  bool use_all_predicted_path{false};
+  bool publish_debug_marker{false};
   // drivable area expansion
-  double drivable_area_right_bound_offset;
-  double drivable_area_left_bound_offset;
+  double drivable_area_right_bound_offset{0.0};
+  double drivable_area_left_bound_offset{0.0};
 };
 
 struct LaneChangeStatus

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -198,6 +198,9 @@ bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_p
 
 bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon);
 
+bool calcObjectPolygon(
+  const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon);
+
 PredictedPath resamplePredictedPath(
   const PredictedPath & input_path, const double resolution, const double duration);
 
@@ -416,13 +419,13 @@ std::vector<PredictedPath> getPredictedPathFromObj(
 Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object);
 
 bool getEgoExpectedPoseAndConvertToPolygon(
-  const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose,
+  const Pose & current_pose, const PredictedPath & pred_path,
   tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time,
-  const VehicleInfo & ego_info);
+  const VehicleInfo & ego_info, Pose & expected_pose, std::string & failed_reason);
 
 bool getObjectExpectedPoseAndConvertToPolygon(
-  const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose,
-  Polygon2d & obj_polygon, const double & check_current_time);
+  const PredictedPath & pred_path, const PredictedObject & object, Polygon2d & obj_polygon,
+  const double & check_current_time, Pose & expected_pose, std::string & failed_reason);
 
 bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose);
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -227,10 +227,10 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
-  p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 3.0);
+  p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 2.0);
   p.longitudinal_distance_min_threshold =
     declare_parameter("longitudinal_distance_min_threshold", 3.0);
-  p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -1.0);
+  p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -0.5);
   p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration", -1.0);
   p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 2.0);
   p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0);
@@ -349,14 +349,15 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.lane_changing_duration = dp("lane_changing_duration", 4.0);
   p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0);
   p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0);
-  p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 8.3);
+  p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 5.6);
   p.prediction_time_resolution = dp("prediction_time_resolution", 0.5);
   p.maximum_deceleration = dp("maximum_deceleration", 1.0);
   p.lane_change_sampling_num = dp("lane_change_sampling_num", 10);
   p.abort_lane_change_velocity_thresh = dp("abort_lane_change_velocity_thresh", 0.5);
   p.abort_lane_change_angle_thresh =
     dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0));
   p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3);
+  p.prepare_phase_ignore_target_speed_thresh = dp("prepare_phase_ignore_target_speed_thresh", 0.1);
   p.enable_abort_lane_change = dp("enable_abort_lane_change", true);
   p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true);
   p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true);
@@ -733,7 +734,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
 
   if (isForcedCandidatePath()) {
     for (auto & path_point : path_candidate->points) {
-      path_point.point.longitudinal_velocity_mps = 1.0;
+      path_point.point.longitudinal_velocity_mps = 0.0;
     }
   }
 

planning/behavior_path_planner/src/scene_module/lane_change/util.cpp

@@ -367,13 +367,13 @@ bool isLaneChangePathSafe(
   const auto & enable_collision_check_at_prepare_phase =
     lane_change_parameters.enable_collision_check_at_prepare_phase;
   const auto & lane_changing_duration = lane_change_parameters.lane_changing_duration;
-  const double check_start_time =
-    (enable_collision_check_at_prepare_phase) ? 0.0 : lane_change_prepare_duration;
   const double check_end_time = lane_change_prepare_duration + lane_changing_duration;
   constexpr double ego_predicted_path_min_speed{1.0};
   const auto vehicle_predicted_path = util::convertToPredictedPath(
     path, current_twist, current_pose, static_cast<double>(current_seg_idx), check_end_time,
     time_resolution, acceleration, ego_predicted_path_min_speed);
+  const auto prepare_phase_ignore_target_speed_thresh =
+    lane_change_parameters.prepare_phase_ignore_target_speed_thresh;
 
   const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose);
 
@@ -416,6 +416,12 @@ bool isLaneChangePathSafe(
 
   for (const auto & i : current_lane_object_indices) {
     const auto & obj = dynamic_objects->objects.at(i);
+    const auto object_speed =
+      util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear);
+    const double check_start_time = (enable_collision_check_at_prepare_phase &&
+                                     (object_speed > prepare_phase_ignore_target_speed_thresh))
+                                      ? 0.0
+                                      : lane_change_prepare_duration;
     auto current_debug_data = assignDebugData(obj);
     const auto predicted_paths =
       util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path);
@@ -449,6 +455,12 @@ bool isLaneChangePathSafe(
     const auto predicted_paths =
       util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path);
 
+    const auto object_speed =
+      util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear);
+    const double check_start_time = (enable_collision_check_at_prepare_phase &&
+                                     (object_speed > prepare_phase_ignore_target_speed_thresh))
+                                      ? 0.0
+                                      : lane_change_prepare_duration;
     if (is_object_in_target) {
       for (const auto & obj_path : predicted_paths) {
         if (!util::isSafeInLaneletCollisionCheck(