feat(lane_change): implement terminal lane change feature (autowarefo…

…undation#9592) * implement function to compute terminal lane change path Signed-off-by: mohammad alqudah <[email protected]> * push terminal path to candidate paths if no other valid candidate path is found Signed-off-by: mohammad alqudah <[email protected]> * use terminal path in LC interface planWaitingApproval function Signed-off-by: mohammad alqudah <[email protected]> * set lane changing longitudinal accel to zero for terminal lc path Signed-off-by: mohammad alqudah <[email protected]> * rename function Signed-off-by: mohammad alqudah <[email protected]> * chore: rename codeowners file Signed-off-by: tomoya.kimura <[email protected]> * remove unused member variable prev_approved_path_ Signed-off-by: mohammad alqudah <[email protected]> * refactor stop point insertion for terminal lc path Signed-off-by: mohammad alqudah <[email protected]> * add flag to enable/disable terminal path feature Signed-off-by: mohammad alqudah <[email protected]> * update README Signed-off-by: mohammad alqudah <[email protected]> * add parameter to configure stop point placement Signed-off-by: mohammad alqudah <[email protected]> * compute terminal path only when near terminal start Signed-off-by: mohammad alqudah <[email protected]> * add option to disable feature near goal Signed-off-by: mohammad alqudah <[email protected]> * set default flag value to false Signed-off-by: mohammad alqudah <[email protected]> * add documentation for terminal lane change path Signed-off-by: mohammad alqudah <[email protected]> * ensure actual prepare duration is always above minimum prepare duration threshold Signed-off-by: mohammad alqudah <[email protected]> * explicitly return std::nullopt Signed-off-by: mohammad alqudah <[email protected]> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp Co-authored-by: Zulfaqar Azmi <[email protected]> * fix assignment Signed-off-by: mohammad alqudah <[email protected]> * fix spelling Signed-off-by: mohammad alqudah <[email protected]> * fix merge errors Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Signed-off-by: tomoya.kimura <[email protected]> Co-authored-by: tomoya.kimura <[email protected]> Co-authored-by: Zulfaqar Azmi <[email protected]>
kyoichi-sugahara · Jan 6, 2025 · c118a92 · c118a92
1 parent 294ecfc
commit c118a92
Show file tree

Hide file tree

Showing 16 changed files with 283 additions and 59 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/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

diff --git a/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/...vior_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

diff --git a/...toware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png b/...toware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png
diff --git a/.../autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png b/.../autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png
diff --git a/..._path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/..._path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp
@@ -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};

diff --git a/...r_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/...r_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp
@@ -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};

diff --git a/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.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(

diff --git a/...ath_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/...ath_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.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
 

diff --git a/...ne_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/...ne_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.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};

diff --git a/...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.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.
  *

diff --git a/..._path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/..._path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.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.

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp
@@ -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();

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
@@ -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