feat(lane_change): using frenet planner to generate lane change path when ego near terminal (#9767)

* frenet planner

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

* minor refactoring

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

* adding parameter

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

* Add diff th param

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

* limit curvature for prepare segment

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

* minor refactoring

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

* print average curvature

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

* refactor

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

* filter the path directly

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

* fix some conflicts

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

* include curvature smoothing

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

* document

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

* fix image folder

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

* image size

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

* doxygen

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

* add debug for state

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

* use sign function instead

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

* rename argument

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

* readme

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

* fix failed test due to empty value

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

* add additional note

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

* fix conflict

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

---------

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

Author: zulfaqar-azmi-t4

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -787,6 +787,51 @@ Depending on the space configuration around the Ego vehicle, it is possible that
 
 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.
 
+## Generating Path Using Frenet Planner
+
+!!! warning
+
+    Generating path using Frenet planner applies only when ego is near terminal start
+
+If the ego vehicle is far from the terminal, the lane change module defaults to using the [path shifter](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design/). This ensures that the lane change is completed while the target lane remains a neighbor of the current lane. However, this approach may result in high curvature paths near the terminal, potentially causing long vehicles to deviate from the lane.
+
+To address this, the lane change module provides an option to choose between the path shifter and the [Frenet planner](https://autowarefoundation.github.io/autoware.universe/main/planning/sampling_based_planner/autoware_frenet_planner/). The Frenet planner allows for some flexibility in the lane change endpoint, extending the lane changing end point slightly beyond the current lane's neighbors.
+
+The following table provides comparisons between the planners
+
+<div align="center">
+  <table>
+    <tr>
+      <td align="center">With Path Shifter</td>
+      <td align="center">With Frenet Planner</td>
+    </tr>
+    <tr>
+      <td><img src="./images/terminal_straight_path_shifter.png" alt="Path shifter result at straight lanelets" width="450"></a></td>
+      <td><img src="./images/terminal_straight_frenet.png" alt="Frenet planner result at straight lanelets" width="450"></a></td>
+    </tr>
+    <tr>
+      <td><img src="./images/terminal_branched_path_shifter.png" alt="Path shifter result at branching lanelets" width="450"></a></td>
+      <td><img src="./images/terminal_branched_frenet.png" alt="Frenet planner result at branching lanelets" width="450"></a></td>
+    </tr>
+    <tr>
+      <td><img src="./images/terminal_curved_path_shifter.png" alt="Path shifter result at curved lanelets" width="450"></a></td>
+      <td><img src="./images/terminal_curved_frenet.png" alt="Frenet planner result at curved lanelets" width="450"></a></td>
+    </tr>
+  </table>
+</div>
+
+!!! note
+
+    The planner can be enabled or disabled using the `frenet.enable` flag.
+
+!!! note
+
+    Since only a segment of the target lane is used as input to generate the lane change path, the end pose of the lane change segment may not smoothly connect to the target lane centerline. To address this, increase the value of `frenet.th_curvature_smoothing` to improve the smoothness.
+
+!!! note
+
+    The yaw difference threshold (`frenet.th_yaw_diff`) limits the maximum curvature difference between the end of the prepare segment and the lane change segment. This threshold might prevent the generation of a lane change path when the lane curvature is high. In such cases, you can increase the frenet.th_yaw_diff value. However, note that if the prepare path was initially shifted by other modules, the resultant steering may not be continuous.
+
 ## Aborting a Previously Approved Lane Change
 
 Once the lane change path is approved, there are several situations where we may need to abort the maneuver. The abort process is triggered when any of the following conditions is met
@@ -1008,6 +1053,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `trajectory.max_longitudinal_acc`            | [m/s2] | double | maximum longitudinal acceleration for lane change                                                                      | 1.0                |
 | `trajectory.min_longitudinal_acc`            | [m/s2] | double | maximum longitudinal deceleration for lane change                                                                      | -1.0               |
 | `trajectory.lane_changing_decel_factor`      | [-]    | double | longitudinal deceleration factor during lane changing phase                                                            | 0.5                |
+| `trajectory.th_prepare_curvature`            | [-]    | double | If the maximum curvature of the prepare segment exceeds the threshold, the prepare segment is invalid.                 | 0.03               |
 | `min_length_for_turn_signal_activation`      | [m]    | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length             | 10.0               |
 | `lateral_acceleration.velocity`              | [m/s]  | double | Reference velocity for lateral acceleration calculation (look up table)                                                | [0.0, 4.0, 10.0]   |
 | `lateral_acceleration.min_values`            | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table)                                              | [0.4, 0.4, 0.4]    |
@@ -1057,6 +1103,18 @@ The following parameters are used to configure terminal lane change path feature
 | `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         |
 
+### Generating Lane Changing Path using Frenet Planner
+
+!!! warning
+
+    Only applicable when ego is near terminal start
+
+| Name                            | Unit  | Type   | Description                                                                                                                                         | Default value |
+| :------------------------------ | ----- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- |
+| `frenet.enable`                 | [-]   | bool   | Flag to enable/disable frenet planner when ego is near terminal start.                                                                              | true          |
+| `frenet.th_yaw_diff`            | [deg] | double | If the yaw diff between of the prepare segment's end and lane changing segment's start exceed the threshold , the lane changing segment is invalid. | 10.0          |
+| `frenet.th_curvature_smoothing` | [-]   | double | Filters and appends target path points with curvature below the threshold to candidate path.                                                        | 0.1           |
+
 ### Collision checks
 
 #### Target Objects

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

@@ -29,6 +29,7 @@
         lon_acc_sampling_num: 5
         lat_acc_sampling_num: 3
         lane_changing_decel_factor: 0.5
+        th_prepare_curvature: 0.03 # [/]
 
       # delay lane change
       delay_lane_change:
@@ -37,6 +38,12 @@
         min_road_shoulder_width: 0.5  # [m]
         th_parked_vehicle_shift_ratio: 0.6
 
+      # trajectory generation near terminal using frenet planner
+      frenet:
+        enable: true
+        th_yaw_diff: 10.0  # [deg]
+        th_curvature_smoothing: 0.1 # [/]
+
       # safety check
       safety_check:
         allow_loose_check_for_cancel: true

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png

@@ -140,6 +140,18 @@ class NormalLaneChange : public LaneChangeBase
 
   bool get_lane_change_paths(LaneChangePaths & candidate_paths) const;
 
+  bool get_path_using_frenet(
+    const std::vector<LaneChangePhaseMetrics> & prepare_metrics,
+    const lane_change::TargetObjects & target_objects,
+    const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+    LaneChangePaths & candidate_paths) const;
+
+  bool get_path_using_path_shifter(
+    const std::vector<LaneChangePhaseMetrics> & prepare_metrics,
+    const lane_change::TargetObjects & target_objects,
+    const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+    LaneChangePaths & candidate_paths) const;
+
   bool check_candidate_path_safety(
     const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const;
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png

@@ -269,6 +269,7 @@ struct CommonData
   LanesPolygonPtr lanes_polygon_ptr;
   TransientData transient_data;
   PathWithLaneId current_lanes_path;
+  PathWithLaneId target_lanes_path;
   ModuleType lc_type;
   Direction direction;
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png

@@ -39,6 +39,20 @@ struct MetricsDebug
   double max_lane_changing_length;
 };
 
+struct FrenetStateDebug
+{
+  LaneChangePhaseMetrics prep_metric;
+  frenet_planner::SamplingParameter sampling_parameter;
+  double max_lane_changing_length;
+
+  FrenetStateDebug(
+    LaneChangePhaseMetrics prep_metric, frenet_planner::SamplingParameter sampling_param,
+    const double max_len)
+  : prep_metric(prep_metric), sampling_parameter(sampling_param), max_lane_changing_length(max_len)
+  {
+  }
+};
+
 struct Debug
 {
   std::string module_type;
@@ -52,6 +66,7 @@ struct Debug
   lanelet::ConstLanelets target_lanes;
   lanelet::ConstLanelets target_backward_lanes;
   std::vector<MetricsDebug> lane_change_metrics;
+  std::vector<FrenetStateDebug> frenet_states;
   double collision_check_object_debug_lifetime{0.0};
   double distance_to_end_of_current_lane{std::numeric_limits<double>::max()};
   double distance_to_lane_change_finished{std::numeric_limits<double>::max()};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png

@@ -113,6 +113,13 @@ struct SafetyParameters
   CollisionCheckParameters collision_check{};
 };
 
+struct FrenetPlannerParameters
+{
+  bool enable{true};
+  double th_yaw_diff_deg{10.0};
+  double th_curvature_smoothing{0.1};
+};
+
 struct TrajectoryParameters
 {
   double max_prepare_duration{4.0};
@@ -124,6 +131,7 @@ struct TrajectoryParameters
   double th_lane_changing_length_diff{0.5};
   double min_lane_changing_velocity{5.6};
   double lane_changing_decel_factor{0.5};
+  double th_prepare_curvature{0.03};
   int lon_acc_sampling_num{10};
   int lat_acc_sampling_num{10};
   LateralAccelerationMap lat_acc_map{};
@@ -151,6 +159,7 @@ struct Parameters
   CancelParameters cancel{};
   DelayParameters delay{};
   TerminalPathParameters terminal_path{};
+  FrenetPlannerParameters frenet{};
 
   // lane change parameters
   double backward_lane_length{200.0};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png

@@ -19,27 +19,60 @@
 #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
+#include <autoware_frenet_planner/structures.hpp>
+
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <utility>
 #include <vector>
 
 namespace autoware::behavior_path_planner::lane_change
 {
+enum class PathType { ConstantJerk = 0, FrenetPlanner };
+
 using autoware::behavior_path_planner::TurnSignalInfo;
 using tier4_planning_msgs::msg::PathWithLaneId;
+struct TrajectoryGroup
+{
+  PathWithLaneId prepare;
+  PathWithLaneId target_lane_ref_path;
+  std::vector<double> target_lane_ref_path_dist;
+  LaneChangePhaseMetrics prepare_metric;
+  frenet_planner::Trajectory lane_changing;
+  frenet_planner::FrenetState initial_state;
+  double max_lane_changing_length{0.0};
+
+  TrajectoryGroup() = default;
+  TrajectoryGroup(
+    PathWithLaneId prepare, PathWithLaneId target_lane_ref_path,
+    std::vector<double> target_lane_ref_path_dist, LaneChangePhaseMetrics prepare_metric,
+    frenet_planner::Trajectory lane_changing, frenet_planner::FrenetState initial_state,
+    const double max_lane_changing_length)
+  : prepare(std::move(prepare)),
+    target_lane_ref_path(std::move(target_lane_ref_path)),
+    target_lane_ref_path_dist(std::move(target_lane_ref_path_dist)),
+    prepare_metric(prepare_metric),
+    lane_changing(std::move(lane_changing)),
+    initial_state(initial_state),
+    max_lane_changing_length(max_lane_changing_length)
+  {
+  }
+};
+
 struct Path
 {
+  Info info;
   PathWithLaneId path;
   ShiftedPath shifted_path;
-  Info info;
+  TrajectoryGroup frenet_path;
+  PathType type = PathType::ConstantJerk;
 };
 
 struct Status
 {
   Path lane_change_path;
   bool is_safe{false};
   bool is_valid_path{false};
-  double start_distance{0.0};
 };
 }  // namespace autoware::behavior_path_planner::lane_change
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png

@@ -11,7 +11,6 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-
 #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_
 #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_
 
@@ -26,6 +25,7 @@ namespace autoware::behavior_path_planner::utils::lane_change
 {
 using behavior_path_planner::LaneChangePath;
 using behavior_path_planner::lane_change::CommonDataPtr;
+using behavior_path_planner::lane_change::TrajectoryGroup;
 
 /**
  * @brief Generates a prepare segment for a lane change maneuver.
@@ -98,5 +98,65 @@ LaneChangePath construct_candidate_path(
   const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment,
   const PathWithLaneId & target_lane_reference_path,
   const std::vector<std::vector<int64_t>> & sorted_lane_ids);
+
+/**
+ * @brief Generates candidate trajectories in the Frenet frame for a lane change maneuver.
+ *
+ * This function computes a set of candidate trajectories for the ego vehicle in the Frenet frame,
+ * based on the provided metrics, target lane reference path, and preparation segments. It ensures
+ * that the generated trajectories adhere to specified constraints, such as lane boundaries and
+ * velocity limits, while optimizing for smoothness and curvature.
+ *
+ * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient
+ * information.
+ * @param prev_module_path The previous module's path used as a base for preparation segments.
+ * @param prep_metric Metrics for the preparation phase, including path length and velocity.
+ *
+ * @return std::vector<lane_change::TrajectoryGroup> A vector of trajectory groups representing
+ * valid lane change candidates, each containing the prepare segment, target reference path, and
+ * Frenet trajectory.
+ */
+std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path,
+  const std::vector<LaneChangePhaseMetrics> & prep_metrics);
+
+/**
+ * @brief Constructs a lane change path candidate based on a Frenet trajectory group.
+ *
+ * This function generates a candidate lane change path by converting a Frenet trajectory group
+ * into a shifted path and combining it with a prepare segment. It validates the path's feasibility
+ * by ensuring yaw differences are within allowable thresholds and calculates lane change
+ * information, such as acceleration, velocity, and duration.
+ *
+ * @param trajectory_group A Frenet trajectory group containing the prepared path and Frenet
+ * trajectory data.
+ * @param common_data_ptr Shared pointer to CommonData providing parameters and constraints for lane
+ * changes.
+ * @param sorted_lane_ids A vector of sorted lane IDs used to update the lane IDs in the path.
+ *
+ * @return std::optional<LaneChangePath> The constructed candidate lane change path if valid, or
+ *         std::nullopt if the path is not feasible.
+ *
+ * @throws std::logic_error If the yaw difference exceeds the threshold, or other critical errors
+ * occur.
+ */
+std::optional<LaneChangePath> get_candidate_path(
+  const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids);
+
+/**
+ * @brief Appends the target lane reference path to the candidate lane change path.
+ *
+ * This function extends the lane change candidate path by appending points from the
+ * target lane reference path beyond the lane change end position. The appending process
+ * is constrained by a curvature threshold to ensure smooth transitions and avoid sharp turns.
+ *
+ * @param frenet_candidate The candidate lane change path to which the target reference path is
+ * appended. This includes the lane change path and associated Frenet trajectory data.
+ * @param th_curvature_smoothing A threshold for the allowable curvature during the extension
+ * process. Points with curvature exceeding this threshold are excluded.
+ */
+void append_target_ref_to_candidate(LaneChangePath & frenet_candidate, const double th_curvature);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
+
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_