fix(lane_change): insert stop for current lanes object (RT0-33761) (#9070)

* RT0-33761 fix lc insert stop for current lanes object

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

* fix wrong value used for comparison

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

* ignore current lane object that is not on ego's path

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

* remove print

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

* update readme

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

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

Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>

* revert is_within_vel_th removal

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

* fix flowchart too wide

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

* rename variable in has_blocking_target_object_for_stopping

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

* Add docstring and rename function

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

* change color

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

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>

Author: zulfaqar-azmi-t4

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -195,7 +195,7 @@ if (max_lane_change_length >  ego's distance to the end of the current lanes.) t
   stop
 endif
 
-if ego is stuck in the current lanes then (yes)
+if (ego is stuck in the current lanes) then (yes)
   :Return **sampled acceleration values**;
   stop
 else (no)
@@ -540,35 +540,106 @@ The following figure illustrates when the lane is blocked in multiple lane chang
 
 ![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png)
 
-#### Stopping position when an object exists ahead
+### Stopping behavior
 
-When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change.
-The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case.
+The stopping behavior of the ego vehicle is determined based on various factors, such as the number of lane changes required, the presence of obstacles, and the position of blocking objects in relation to the lane change plan. The objective is to choose a suitable stopping point that allows for a safe and effective lane change while adapting to different traffic scenarios.
 
-##### When the ego vehicle is near the end of the lane change
+The following flowchart and subsections explain the conditions for deciding where to insert a stop point when an obstacle is ahead.
 
-Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead.
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #WHITE
+
+title Inserting Stop Point
+
+start
+if (number of lane changes is zero?) then (<color:green><b>YES</b></color>)
+stop
+else (<color:red><b>NO</b></color>)
+endif
+
+if (do we want to insert stop point in current lanes?) then (<color:red><b>NO</b></color>)
+#LightPink:Insert stop point at next lane change terminal start.;
+stop
+else (<color:green><b>YES</b></color>)
+endif
+
+if (Is there leading object in the current lanes that blocks ego's path?) then (<color:red><b>NO</b></color>)
+#LightPink:Insert stop point at terminal stop.;
+stop
+else (<color:green><b>YES</b></color>)
+endif
+
+if (Blocking object's position is after target lane's start position?) then (<color:red><b>NO</b></color>)
+#LightPink:Insert stop at the target lane's start position.;
+stop
+else (<color:green><b>YES</b></color>)
+endif
+
+if (Blocking object's position is before terminal stop position?) then (<color:red><b>NO</b></color>)
+#LightPink:Insert stop at the terminal stop position;
+stop
+else (<color:green><b>YES</b></color>)
+endif
+
+if (Are there target lane objects between the ego and the blocking object?) then (<color:red><b>NO</b></color>)
+#LightGreen:Insert stop behind the blocking object;
+stop
+else (<color:green><b>YES</b></color>)
+#LightPink:Insert stop at terminal stop;
+stop
+@enduml
+```
+
+#### Ego vehicle's stopping position when an object exists ahead
+
+When the ego vehicle encounters an obstacle ahead, it stops while maintaining a safe distance to prepare for a possible lane change. The exact stopping position depends on factors like whether the target lane is clear or if the lane change needs to be delayed. The following explains how different stopping scenarios are handled:
+
+##### When the near the end of the lane change
+
+Whether the target lane has obstacles or is clear, the ego vehicle stops while keeping a safe distance from the obstacle ahead, ensuring there is enough room for the lane change.
 
 ![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg)
 
 ![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg)
 
 ##### When the ego vehicle is not near the end of the lane change
 
-If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead.
+The ego vehicle stops while maintaining a safe distance from the obstacle ahead, ensuring there is enough space for a lane change.
+
+![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg)
+
+#### Ego vehicle's stopping position when an object exists in the lane changing section
+
+If there are objects within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without maintaining the usual distance for a lane change.
+
+##### When near the end of the lane change
+
+Regardless of whether there are obstacles in the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead, allowing for the lane change.
+
+![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg)
+
+![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg)
+
+##### When not near the end of the lane change
+
+If there are no obstacles in the lane change section of the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead to accommodate the lane change.
 
 ![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg)
 
-If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead.
+If there are obstacles within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without keeping the usual distance needed for a lane change.
 
 ![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg)
 
-##### When the target lane is far away
+#### When the target lane is far away
 
-When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead.
+If the target lane for the lane change is far away and not next to the current lane, the ego vehicle stops closer to the obstacle ahead, as maintaining the usual distance for a lane change is not necessary.
 
 ![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg)
 
+![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg)
+
 ### Lane Change When Stuck
 
 The ego vehicle is considered stuck if it is stopped and meets any of the following conditions:

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

@@ -70,7 +70,9 @@ class NormalLaneChange : public LaneChangeBase
 
   void extendOutputDrivableArea(BehaviorModuleOutput & output) const override;
 
-  void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override;
+  void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override;
+
+  void insert_stop_point_on_current_lanes(PathWithLaneId & path);
 
   PathWithLaneId getReferencePath() const override;
 
@@ -209,7 +211,7 @@ class NormalLaneChange : public LaneChangeBase
 
   std::pair<double, double> calcCurrentMinMaxAcceleration() const;
 
-  void setStopPose(const Pose & stop_pose);
+  void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path);
 
   void updateStopTime();
 

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

@@ -127,7 +127,7 @@ class LaneChangeBase
 
   virtual void updateSpecialData() {}
 
-  virtual void insertStopPoint(
+  virtual void insert_stop_point(
     [[maybe_unused]] const lanelet::ConstLanelets & lanelets,
     [[maybe_unused]] PathWithLaneId & path)
   {

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

@@ -266,6 +266,48 @@ ExtendedPredictedObjects transform_to_extended_objects(
 double get_distance_to_next_regulatory_element(
   const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false,
   const bool ignore_intersection = false);
+
+/**
+ * @brief Calculates the minimum distance to a stationary object in the current lanes.
+ *
+ * This function determines the closest distance from the ego vehicle to a stationary object
+ * in the current lanes. It checks if the object is within the stopping criteria based on its
+ * velocity and calculates the distance while accounting for the object's size. Only objects
+ * positioned after the specified distance to the target lane's start are considered.
+ *
+ * @param common_data_ptr Pointer to the common data structure containing parameters for lane
+ * change.
+ * @param filtered_objects A collection of objects filtered by lanes, including those in the current
+ * lane.
+ * @param dist_to_target_lane_start The distance to the start of the target lane from the ego
+ * vehicle.
+ * @param path The current path of the ego vehicle, containing path points and lane information.
+ * @return The minimum distance to a stationary object in the current lanes. If no valid object is
+ * found, returns the maximum possible double value.
+ */
+double get_min_dist_to_current_lanes_obj(
+  const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
+  const double dist_to_target_lane_start, const PathWithLaneId & path);
+
+/**
+ * @brief Checks if there is an object in the target lane that influences the decision to insert a
+ * stop point.
+ *
+ * This function determines whether any objects exist in the target lane that would affect
+ * the decision to place a stop point behind a blocking object in the current lane.
+ *
+ * @param common_data_ptr Pointer to the common data structure containing parameters for the lane
+ * change.
+ * @param filtered_objects A collection of objects filtered by lanes, including those in the target
+ * lane.
+ * @param stop_arc_length The arc length at which the ego vehicle is expected to stop.
+ * @param path The current path of the ego vehicle, containing path points and lane information.
+ * @return true if there is an object in the target lane that influences the stop point decision;
+ * otherwise, false.
+ */
+bool has_blocking_target_object(
+  const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
+  const double stop_arc_length, const PathWithLaneId & path);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -154,7 +154,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 
   BehaviorModuleOutput out = getPreviousModuleOutput();
 
-  module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path);
+  module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path);
   out.turn_signal_info = module_type_->get_current_turn_signal_info();
 
   const auto & lane_change_debug = module_type_->getDebugData();