fix(lane_change): do not cancel when approaching terminal start (auto…

…warefoundation#8381)

* do not cancel if ego vehicle approaching terminal start

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

* Insert stop point if object is coming from rear

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

* minor edit to fix conflict

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

* rename function

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

---------

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

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -92,6 +92,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isAbleToReturnCurrentLane() const override;
 
+  bool is_near_terminal() const final;
+
   bool isEgoOnPreparePhase() const override;
 
   bool isAbleToStopSafely() const override;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp

@@ -93,6 +93,8 @@ class LaneChangeBase
 
   virtual bool isAbleToReturnCurrentLane() const = 0;
 
+  virtual bool is_near_terminal() const = 0;
+
   virtual LaneChangePath getLaneChangePath() const = 0;
 
   virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp

@@ -19,6 +19,7 @@
 namespace autoware::behavior_path_planner::utils::lane_change::calculation
 {
 using behavior_path_planner::lane_change::CommonDataPtr;
+using behavior_path_planner::lane_change::LCParamPtr;
 
 /**
  * @brief Calculates the distance from the ego vehicle to the terminal point.
@@ -40,6 +41,23 @@ double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr);
 double calc_dist_from_pose_to_terminal_end(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
   const Pose & src_pose);
+
+/**
+ * @brief Calculates the minimum stopping distance to terminal start.
+ *
+ * This function computes the minimum stopping distance to terminal start based on the
+ * minimum lane changing velocity and the minimum longitudinal acceleration. It then
+ * compares this calculated distance with a pre-defined backward length buffer parameter
+ * and returns the larger of the two values to ensure safe lane changing.
+ *
+ * @param lc_param_ptr Shared pointer to an LCParam structure, which should include:
+ *  - `minimum_lane_changing_velocity`: The minimum velocity required for lane changing.
+ *  - `min_longitudinal_acc`: The minimum longitudinal acceleration used for deceleration.
+ *  - `backward_length_buffer_for_end_of_lane`: A predefined backward buffer length parameter.
+ *
+ * @return The required backward buffer distance in meters.
+ */
+double calc_stopping_distance(const LCParamPtr & lc_param_ptr);
 }  // namespace autoware::behavior_path_planner::utils::lane_change::calculation
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -247,6 +247,15 @@ bool LaneChangeInterface::canTransitFailureState()
     return true;
   }
 
+  if (module_type_->is_near_terminal()) {
+    log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change");
+
+    if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) {
+      log_debug_throttled("Module require stopping");
+    }
+    return false;
+  }
+
   if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) {
     if (module_type_->isStoppedAtRedTrafficLight()) {
       log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change");

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -769,6 +769,31 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const
   return true;
 }
 
+bool NormalLaneChange::is_near_terminal() const
+{
+  const auto & current_lanes = common_data_ptr_->lanes_ptr->current;
+
+  if (current_lanes.empty()) {
+    return true;
+  }
+
+  const auto & current_lanes_terminal = current_lanes.back();
+  const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr;
+  const auto direction = common_data_ptr_->direction;
+  const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr;
+  const auto min_lane_changing_distance = calcMinimumLaneChangeLength(
+    route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction);
+
+  const auto backward_buffer = calculation::calc_stopping_distance(lc_param_ptr);
+
+  const auto min_lc_dist_with_buffer =
+    backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer;
+  const auto dist_from_ego_to_terminal_end =
+    calculation::calc_ego_dist_to_terminal_end(common_data_ptr_);
+
+  return dist_from_ego_to_terminal_end < min_lc_dist_with_buffer;
+}
+
 bool NormalLaneChange::isEgoOnPreparePhase() const
 {
   const auto & start_position = status_.lane_change_path.info.shift_line.start.position;

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

@@ -1,3 +1,4 @@
+
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
@@ -42,4 +43,15 @@ double calc_dist_from_pose_to_terminal_end(
   }
   return utils::getDistanceToEndOfLane(src_pose, lanes);
 }
+
+double calc_stopping_distance(const LCParamPtr & lc_param_ptr)
+{
+  // v^2 = u^2 + 2ad
+  const auto min_lc_vel = lc_param_ptr->minimum_lane_changing_velocity;
+  const auto min_lon_acc = lc_param_ptr->min_longitudinal_acc;
+  const auto min_back_dist = std::abs((min_lc_vel * min_lc_vel) / (2 * min_lon_acc));
+
+  const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane;
+  return std::max(min_back_dist, param_back_dist);
+}
 }  // namespace autoware::behavior_path_planner::utils::lane_change::calculation