feat(lane_change): implement terminal lane change feature (#9592)

* implement function to compute terminal lane change path

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* push terminal path to candidate paths if no other valid candidate path is found

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* use terminal path in LC interface planWaitingApproval function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* set lane changing longitudinal accel to zero for terminal lc path

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* rename function

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* chore: rename codeowners file

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* remove unused member variable prev_approved_path_

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* refactor stop point insertion for terminal lc path

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add flag to enable/disable terminal path feature

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* update README

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add parameter to configure stop point placement

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* compute terminal path only when near terminal start

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add option to disable feature near goal

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* set default flag value to false

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add documentation for terminal lane change path

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* ensure actual prepare duration is always above minimum prepare duration threshold

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* explicitly return std::nullopt

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

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

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* fix assignment

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix spelling

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix merge errors

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

---------

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

Author: 3 people

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -874,6 +874,16 @@ endif
 @enduml
 ```
 
+## Terminal Lane Change Path
+
+Depending on the space configuration around the Ego vehicle, it is possible that a valid LC path cannot be generated. If that happens, then Ego will get stuck at `terminal_start` and will be not be able to proceed. Therefore we introduced the terminal LC path feature; when ego gets near to the terminal point (dist to `terminal_start` is less than the maximum lane change length) a terminal lane changing path will be computed starting from the terminal start point on the current lane and connects to the target lane. The terminal path only needs to be computed once in each execution of LC module. If no valid candidate paths are found in the path generation process, then the terminal path will be used as a fallback candidate path, the safety of the terminal path is not ensured and therefore it can only be force approved. The following images illustrate the expected behavior without and with the terminal path feature respectively:
+
+![no terminal path](./images/lane_change-no_terminal_path.png)
+
+![terminal path](./images/lane_change-terminal_path.png)
+
+Additionally if terminal path feature is enabled and path is computed, stop point placement can be configured to be at the edge of the current lane instead of at the `terminal_start` position, as indicated by the dashed red line in the image above.
+
 ## Parameters
 
 ### Essential lane change parameters
@@ -935,6 +945,16 @@ The following parameters are used to judge lane change completion.
 | `delay_lane_change.min_road_shoulder_width`       | [m]  | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5           |
 | `delay_lane_change.th_parked_vehicle_shift_ratio` | [-]  | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked             | 0.6           |
 
+### Terminal Lane Change Path
+
+The following parameters are used to configure terminal lane change path feature.
+
+| Name                              | Unit | Type | Description                                                               | Default value |
+| :-------------------------------- | ---- | ---- | ------------------------------------------------------------------------- | ------------- |
+| `terminal_path.enable`            | [-]  | bool | Flag to enable/disable terminal path feature                              | true          |
+| `terminal_path.disable_near_goal` | [-]  | bool | Flag to disable terminal path feature if ego is near goal                 | true          |
+| `terminal_path.stop_at_boundary`  | [-]  | bool | If true, ego will stop at current lane boundary instead of middle of lane | false         |
+
 ### Collision checks
 
 #### Target Objects

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

@@ -10,6 +10,12 @@
       # turn signal
       min_length_for_turn_signal_activation: 10.0 # [m]
 
+      # terminal path
+      terminal_path:
+        enable: true
+        disable_near_goal: true
+        stop_at_boundary: false
+
       # trajectory generation
       trajectory:
         max_prepare_duration: 4.0

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png

@@ -104,6 +104,8 @@ class LaneChangeBase
 
   virtual LaneChangePath getLaneChangePath() const = 0;
 
+  virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0;
+
   virtual bool isEgoOnPreparePhase() const = 0;
 
   virtual bool isRequiredStop(const bool is_trailing_object) = 0;
@@ -130,7 +132,7 @@ class LaneChangeBase
 
   virtual void insert_stop_point(
     [[maybe_unused]] const lanelet::ConstLanelets & lanelets,
-    [[maybe_unused]] PathWithLaneId & path)
+    [[maybe_unused]] PathWithLaneId & path, [[maybe_unused]] const bool is_waiting_approval = false)
   {
   }
 
@@ -285,8 +287,7 @@ class LaneChangeBase
   FilteredLanesObjects filtered_objects_{};
   BehaviorModuleOutput prev_module_output_{};
   PoseWithDetailOpt lane_change_stop_pose_{std::nullopt};
-
-  PathWithLaneId prev_approved_path_;
+  mutable std::optional<LaneChangePath> terminal_lane_change_path_{std::nullopt};
 
   int unsafe_hysteresis_count_{0};
   bool is_abort_path_approved_{false};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png

@@ -129,8 +129,6 @@ class LaneChangeInterface : public SceneModuleInterface
 
   mutable MarkerArray virtual_wall_marker_;
 
-  std::unique_ptr<PathWithLaneId> prev_approved_path_;
-
   void clearAbortApproval() { is_abort_path_approved_ = false; }
 
   bool is_abort_path_approved_{false};

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

@@ -65,13 +65,18 @@ class NormalLaneChange : public LaneChangeBase
 
   LaneChangePath getLaneChangePath() const override;
 
+  BehaviorModuleOutput getTerminalLaneChangePath() const override;
+
   BehaviorModuleOutput generateOutput() override;
 
   void extendOutputDrivableArea(BehaviorModuleOutput & output) const override;
 
-  void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override;
+  void insert_stop_point(
+    const lanelet::ConstLanelets & lanelets, PathWithLaneId & path,
+    const bool is_waiting_approval = false) override;
 
-  void insert_stop_point_on_current_lanes(PathWithLaneId & path);
+  void insert_stop_point_on_current_lanes(
+    PathWithLaneId & path, const bool is_waiting_approval = false);
 
   PathWithLaneId getReferencePath() const override;
 
@@ -137,6 +142,8 @@ class NormalLaneChange : public LaneChangeBase
   bool check_candidate_path_safety(
     const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const;
 
+  std::optional<PathWithLaneId> compute_terminal_lane_change_path() const;
+
   bool isValidPath(const PathWithLaneId & path) const override;
 
   PathSafetyStatus isLaneChangePathSafe(

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

@@ -128,6 +128,22 @@ struct Info
     terminal_lane_changing_velocity = _lc_metrics.velocity;
     shift_line = _shift_line;
   }
+
+  void set_prepare(const PhaseMetrics & _prep_metrics)
+  {
+    longitudinal_acceleration.prepare = _prep_metrics.actual_lon_accel;
+    velocity.prepare = _prep_metrics.velocity;
+    duration.prepare = _prep_metrics.duration;
+    length.prepare = _prep_metrics.length;
+  }
+
+  void set_lane_changing(const PhaseMetrics & _lc_metrics)
+  {
+    longitudinal_acceleration.lane_changing = _lc_metrics.actual_lon_accel;
+    velocity.lane_changing = _lc_metrics.velocity;
+    duration.lane_changing = _lc_metrics.duration;
+    length.lane_changing = _lc_metrics.length;
+  }
 };
 
 struct TargetLaneLeadingObjects
@@ -219,6 +235,8 @@ struct TransientData
 
   double target_lane_length{std::numeric_limits<double>::min()};
 
+  double dist_to_target_end{std::numeric_limits<double>::max()};
+
   lanelet::ArcCoordinates current_lanes_ego_arc;  // arc coordinates of ego pose along current lanes
   lanelet::ArcCoordinates target_lanes_ego_arc;   // arc coordinates of ego pose along target lanes
 

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

@@ -137,12 +137,20 @@ struct DelayParameters
   double th_parked_vehicle_shift_ratio{0.6};
 };
 
+struct TerminalPathParameters
+{
+  bool enable{false};
+  bool disable_near_goal{false};
+  bool stop_at_boundary{false};
+};
+
 struct Parameters
 {
   TrajectoryParameters trajectory{};
   SafetyParameters safety{};
   CancelParameters cancel{};
   DelayParameters delay{};
+  TerminalPathParameters terminal_path{};
 
   // lane change parameters
   double backward_lane_length{200.0};

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

@@ -67,6 +67,9 @@ double calc_dist_to_last_fit_width(
   const lanelet::ConstLanelets & lanelets, const Pose & src_pose,
   const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1);
 
+double calc_dist_to_last_fit_width(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin = 0.1);
+
 /**
  * @brief Calculates the maximum preparation longitudinal distance for lane change.
  *

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

@@ -48,7 +48,7 @@ using behavior_path_planner::lane_change::CommonDataPtr;
  */
 bool get_prepare_segment(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path,
-  const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment);
+  const double prep_length, PathWithLaneId & prepare_segment);
 
 /**
  * @brief Generates the candidate path for a lane change maneuver.

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

@@ -43,8 +43,7 @@ LaneChangeInterface::LaneChangeInterface(
   std::unique_ptr<LaneChangeBase> && module_type)
 : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{std::move(parameters)},
-  module_type_{std::move(module_type)},
-  prev_approved_path_{std::make_unique<PathWithLaneId>()}
+  module_type_{std::move(module_type)}
 {
   module_type_->setTimeKeeper(getTimeKeeper());
   logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr());
@@ -109,7 +108,6 @@ BehaviorModuleOutput LaneChangeInterface::plan()
 
   auto output = module_type_->generateOutput();
   path_reference_ = std::make_shared<PathWithLaneId>(output.reference_path);
-  *prev_approved_path_ = getPreviousModuleOutput().path;
 
   stop_pose_ = module_type_->getStopPose();
 
@@ -155,11 +153,9 @@ BehaviorModuleOutput LaneChangeInterface::plan()
 
 BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 {
-  BehaviorModuleOutput out = getPreviousModuleOutput();
+  BehaviorModuleOutput out = module_type_->getTerminalLaneChangePath();
 
-  *prev_approved_path_ = out.path;
-
-  module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path);
+  module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path, true);
   out.turn_signal_info = module_type_->get_current_turn_signal_info();
 
   const auto & lane_change_debug = module_type_->getDebugData();

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

@@ -242,6 +242,13 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
   // debug marker
   p.publish_debug_marker = getOrDeclareParameter<bool>(*node, parameter("publish_debug_marker"));
 
+  // terminal lane change path
+  p.terminal_path.enable = getOrDeclareParameter<bool>(*node, parameter("terminal_path.enable"));
+  p.terminal_path.disable_near_goal =
+    getOrDeclareParameter<bool>(*node, parameter("terminal_path.disable_near_goal"));
+  p.terminal_path.stop_at_boundary =
+    getOrDeclareParameter<bool>(*node, parameter("terminal_path.stop_at_boundary"));
+
   // validation of safety check parameters
   // if loose check is not enabled, lane change module will keep on chattering and canceling, and
   // false positive situation might  occur