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

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -884,6 +884,39 @@ 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. 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. 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="./image/terminal_straight_path_shifter.png" alt="Path shifter result at straight lanelets" width="300"></a></td>
+      <td><img src="./image/terminal_straight_frenet.png" alt="Frenet planner result at straight lanelets" width="300"></a></td>
+    </tr>
+    <tr>
+      <td><img src="./image/terminal_branched_path_shifter.png" alt="Path shifter result at branching lanelets" width="300"></a></td>
+      <td><img src="./image/terminal_branched_frenet.png" alt="Frenet planner result at branching lanelets" width="300"></a></td>
+    </tr>
+    <tr>
+      <td><img src="./image/terminal_curved_path_shifter.png" alt="Path shifter result at curved lanelets" width="300"></a></td>
+      <td><img src="./image/terminal_curved_frenet.png" alt="Frenet planner result at curved lanelets" width="300"></a></td>
+    </tr>
+  </table>
+</div>
+
 ## Parameters
 
 ### Essential lane change parameters
@@ -906,6 +939,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]    |
@@ -955,6 +989,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/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -139,6 +139,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/include/autoware/behavior_path_lane_change_module/structs/data.hpp

@@ -268,6 +268,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/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp

@@ -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/include/autoware/behavior_path_lane_change_module/structs/path.hpp

@@ -19,27 +19,57 @@
 #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;
+
+  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)
+  : 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)
+  {
+  }
+};
+
 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/include/autoware/behavior_path_lane_change_module/utils/path.hpp

@@ -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,66 @@ 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 metrics A collection of metrics defining lane change phases, such as lengths and
+ * velocities.
+ *
+ * @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> & 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_

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

@@ -24,6 +24,8 @@
 
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware_frenet_planner/structures.hpp>
+#include <autoware_sampler_common/transform/spline_transform.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -56,6 +58,7 @@ using behavior_path_planner::lane_change::LCParamPtr;
 using behavior_path_planner::lane_change::ModuleType;
 using behavior_path_planner::lane_change::PathSafetyStatus;
 using behavior_path_planner::lane_change::TargetLaneLeadingObjects;
+using behavior_path_planner::lane_change::TrajectoryGroup;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
@@ -70,8 +73,9 @@ void set_prepare_velocity(
   PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity);
 
 std::vector<int64_t> replaceWithSortedIds(
-  const std::vector<int64_t> & original_lane_ids,
-  const std::vector<std::vector<int64_t>> & sorted_lane_ids);
+  const std::vector<int64_t> & current_lane_ids,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids, std::vector<int64_t> & prev_lane_ids,
+  std::vector<int64_t> & prev_sorted_lane_ids);
 
 std::vector<std::vector<int64_t>> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr);
 
@@ -432,5 +436,11 @@ std::vector<std::vector<PoseWithVelocityStamped>> convert_to_predicted_paths(
  * @return true if the pose is within the target or target neighbor polygons, false otherwise.
  */
 bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose);
+
+std::vector<PoseWithVelocityStamped> convert_to_predicted_path(
+  const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate,
+  [[maybe_unused]] const size_t deceleration_sampling_num);
+
+double calc_limit(const CommonDataPtr & common_data_ptr, const Pose & lc_end_pose);
 }  // 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/package.xml

@@ -23,6 +23,7 @@
 
   <depend>autoware_behavior_path_planner</depend>
   <depend>autoware_behavior_path_planner_common</depend>
+  <depend>autoware_frenet_planner</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_rtc_interface</depend>
   <depend>autoware_universe_utils</depend>

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -63,156 +63,164 @@
       getOrDeclareParameter<double>(*node, parameter("trajectory.min_lane_changing_velocity"));
     p.trajectory.lane_changing_decel_factor =
       getOrDeclareParameter<double>(*node, parameter("trajectory.lane_changing_decel_factor"));
+    p.trajectory.th_prepare_curvature =
+      getOrDeclareParameter<double>(*node, parameter("trajectory.th_prepare_curvature"));
     p.trajectory.lon_acc_sampling_num =
       getOrDeclareParameter<int>(*node, parameter("trajectory.lon_acc_sampling_num"));
     p.trajectory.lat_acc_sampling_num =
       getOrDeclareParameter<int>(*node, parameter("trajectory.lat_acc_sampling_num"));
 
     const auto max_acc = getOrDeclareParameter<double>(*node, "normal.max_acc");
     p.trajectory.min_lane_changing_velocity = std::min(
       p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.max_prepare_duration);
 
     // validation of trajectory parameters
     if (p.trajectory.lon_acc_sampling_num < 1 || p.trajectory.lat_acc_sampling_num < 1) {
       RCLCPP_FATAL_STREAM(
         node->get_logger().get_child(node_name),
         "lane_change_sampling_num must be positive integer. Given longitudinal parameter: "
           << p.trajectory.lon_acc_sampling_num
           << "Given lateral parameter: " << p.trajectory.lat_acc_sampling_num << std::endl
           << "Terminating the program...");
       exit(EXIT_FAILURE);
     }
 
     // lateral acceleration map
     const auto lateral_acc_velocity =
       getOrDeclareParameter<std::vector<double>>(*node, parameter("lateral_acceleration.velocity"));
     const auto min_lateral_acc = getOrDeclareParameter<std::vector<double>>(
       *node, parameter("lateral_acceleration.min_values"));
     const auto max_lateral_acc = getOrDeclareParameter<std::vector<double>>(
       *node, parameter("lateral_acceleration.max_values"));
     if (
       lateral_acc_velocity.size() != min_lateral_acc.size() ||
       lateral_acc_velocity.size() != max_lateral_acc.size()) {
       RCLCPP_ERROR(
         node->get_logger().get_child(node_name),
         "Lane change lateral acceleration map has invalid size.");
       exit(EXIT_FAILURE);
     }
     for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) {
       p.trajectory.lat_acc_map.add(
         lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i));
     }
   }
 
   // turn signal
   p.min_length_for_turn_signal_activation =
     getOrDeclareParameter<double>(*node, parameter("min_length_for_turn_signal_activation"));
 
   // lane change regulations
   p.regulate_on_crosswalk = getOrDeclareParameter<bool>(*node, parameter("regulation.crosswalk"));
   p.regulate_on_intersection =
     getOrDeclareParameter<bool>(*node, parameter("regulation.intersection"));
   p.regulate_on_traffic_light =
     getOrDeclareParameter<bool>(*node, parameter("regulation.traffic_light"));
 
   // ego vehicle stuck detection
   p.th_stop_velocity = getOrDeclareParameter<double>(*node, parameter("stuck_detection.velocity"));
   p.th_stop_time = getOrDeclareParameter<double>(*node, parameter("stuck_detection.stop_time"));
 
   // safety
   {
     p.safety.enable_loose_check_for_cancel =
       getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
     p.safety.enable_target_lane_bound_check =
       getOrDeclareParameter<bool>(*node, parameter("safety_check.enable_target_lane_bound_check"));
     p.safety.th_stopped_object_velocity = getOrDeclareParameter<double>(
       *node, parameter("safety_check.stopped_object_velocity_threshold"));
     p.safety.lane_expansion_left_offset =
       getOrDeclareParameter<double>(*node, parameter("safety_check.lane_expansion.left_offset"));
     p.safety.lane_expansion_right_offset =
       getOrDeclareParameter<double>(*node, parameter("safety_check.lane_expansion.right_offset"));
 
     // collision check
     p.safety.collision_check.enable_for_prepare_phase_in_general_lanes =
       getOrDeclareParameter<bool>(
         *node, parameter("collision_check.enable_for_prepare_phase.general_lanes"));
     p.safety.collision_check.enable_for_prepare_phase_in_intersection = getOrDeclareParameter<bool>(
       *node, parameter("collision_check.enable_for_prepare_phase.intersection"));
     p.safety.collision_check.enable_for_prepare_phase_in_turns = getOrDeclareParameter<bool>(
       *node, parameter("collision_check.enable_for_prepare_phase.turns"));
     p.safety.collision_check.check_current_lane =
       getOrDeclareParameter<bool>(*node, parameter("collision_check.check_current_lanes"));
     p.safety.collision_check.check_other_lanes =
       getOrDeclareParameter<bool>(*node, parameter("collision_check.check_other_lanes"));
     p.safety.collision_check.use_all_predicted_paths =
       getOrDeclareParameter<bool>(*node, parameter("collision_check.use_all_predicted_paths"));
     p.safety.collision_check.prediction_time_resolution =
       getOrDeclareParameter<double>(*node, parameter("collision_check.prediction_time_resolution"));
     p.safety.collision_check.th_yaw_diff =
       getOrDeclareParameter<double>(*node, parameter("collision_check.yaw_diff_threshold"));
 
     // rss check
     auto set_rss_params = [&](auto & params, const std::string & prefix) {
       params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
         *node, parameter(prefix + ".longitudinal_distance_min_threshold"));
       params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
         *node, parameter(prefix + ".longitudinal_velocity_delta_time"));
       params.front_vehicle_deceleration =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".expected_front_deceleration"));
       params.rear_vehicle_deceleration =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".expected_rear_deceleration"));
       params.rear_vehicle_reaction_time =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".rear_vehicle_reaction_time"));
       params.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
         *node, parameter(prefix + ".rear_vehicle_safety_time_margin"));
       params.lateral_distance_max_threshold =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".lateral_distance_max_threshold"));
     };
     set_rss_params(p.safety.rss_params, "safety_check.execution");
     set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked");
     set_rss_params(p.safety.rss_params_for_abort, "safety_check.cancel");
     set_rss_params(p.safety.rss_params_for_stuck, "safety_check.stuck");
 
     // target object types
     const std::string ns = "lane_change.target_object.";
     p.safety.target_object_types.check_car = getOrDeclareParameter<bool>(*node, ns + "car");
     p.safety.target_object_types.check_truck = getOrDeclareParameter<bool>(*node, ns + "truck");
     p.safety.target_object_types.check_bus = getOrDeclareParameter<bool>(*node, ns + "bus");
     p.safety.target_object_types.check_trailer = getOrDeclareParameter<bool>(*node, ns + "trailer");
     p.safety.target_object_types.check_unknown = getOrDeclareParameter<bool>(*node, ns + "unknown");
     p.safety.target_object_types.check_bicycle = getOrDeclareParameter<bool>(*node, ns + "bicycle");
     p.safety.target_object_types.check_motorcycle =
       getOrDeclareParameter<bool>(*node, ns + "motorcycle");
     p.safety.target_object_types.check_pedestrian =
       getOrDeclareParameter<bool>(*node, ns + "pedestrian");
   }
 
   // lane change parameters
   p.backward_lane_length = getOrDeclareParameter<double>(*node, parameter("backward_lane_length"));
   p.backward_length_buffer_for_end_of_lane =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_end_of_lane"));
   p.backward_length_buffer_for_blocking_object =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_blocking_object"));
   p.backward_length_from_intersection =
     getOrDeclareParameter<double>(*node, parameter("backward_length_from_intersection"));
   p.enable_stopped_vehicle_buffer =
     getOrDeclareParameter<bool>(*node, parameter("enable_stopped_vehicle_buffer"));
 
   if (p.backward_length_buffer_for_end_of_lane < 1.0) {
     RCLCPP_WARN_STREAM(
       node->get_logger().get_child(node_name),
       "Lane change buffer must be more than 1 meter. Modifying the buffer.");
   }
 
   // lane change delay
   p.delay.enable = getOrDeclareParameter<bool>(*node, parameter("delay_lane_change.enable"));
   p.delay.check_only_parked_vehicle =
     getOrDeclareParameter<bool>(*node, parameter("delay_lane_change.check_only_parked_vehicle"));
   p.delay.min_road_shoulder_width =
     getOrDeclareParameter<double>(*node, parameter("delay_lane_change.min_road_shoulder_width"));
   p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter<double>(
     *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio"));
 
+  // trajectory generation near terminal using frenet planner
+  p.frenet.enable = getOrDeclareParameter<bool>(*node, parameter("frenet.enable"));
+  p.frenet.th_yaw_diff_deg = getOrDeclareParameter<double>(*node, parameter("frenet.th_yaw_diff"));
+  p.frenet.th_curvature_smoothing =
+    getOrDeclareParameter<double>(*node, parameter("frenet.th_curvature_smoothing"));
+
   // lane change cancel
   p.cancel.enable_on_prepare_phase =
     getOrDeclareParameter<bool>(*node, parameter("cancel.enable_on_prepare_phase"));
@@ -338,24 +346,34 @@
       parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc);
     updateParam<double>(
       parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor);
+    updateParam<double>(
+      parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature);
     int longitudinal_acc_sampling_num = 0;
     updateParam<int>(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num);
     if (longitudinal_acc_sampling_num > 0) {
       p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num;
     }
 
     int lateral_acc_sampling_num = 0;
     updateParam<int>(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num);
     if (lateral_acc_sampling_num > 0) {
       p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num;
     }
 
     updateParam<double>(
       parameters, ns + "th_prepare_length_diff", p->trajectory.th_prepare_length_diff);
     updateParam<double>(
       parameters, ns + "th_lane_changing_length_diff", p->trajectory.th_lane_changing_length_diff);
   }
 
+  {
+    const std::string ns = "lane_change.frenet.";
+    updateParam<bool>(parameters, ns + "enable", p->frenet.enable);
+    updateParam<double>(parameters, ns + "th_yaw_diff", p->frenet.th_yaw_diff_deg);
+    updateParam<double>(
+      parameters, ns + "th_curvature_smoothing", p->frenet.th_curvature_smoothing);
+  }
+
   {
     const std::string ns = "lane_change.safety_check.lane_expansion.";
     updateParam<double>(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset);