refactor(lane_change): refactor getLaneChangePaths function

planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp

@@ -326,6 +326,9 @@ class RouteHandler
    */
   lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const;
 
+  Pose getPoseFrom2DArcLength(
+    const lanelet::ConstLanelets & lanelet_sequence, const double s) const;
+
 private:
   // MUST
   lanelet::routing::RoutingGraphPtr routing_graph_ptr_;

planning/autoware_route_handler/src/route_handler.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -47,6 +47,8 @@
 {
 namespace
 {
+using autoware::universe_utils::createPoint;
+using autoware::universe_utils::createQuaternionFromYaw;
 using autoware_planning_msgs::msg::LaneletPrimitive;
 using autoware_planning_msgs::msg::Path;
 using geometry_msgs::msg::Pose;
@@ -2035,4 +2037,29 @@
   if (route) return route->shortestPath();
   return {};
 }
+
+Pose RouteHandler::getPoseFrom2DArcLength(
+  const lanelet::ConstLanelets & lanelet_sequence, const double s) const
+{
+  double accumulated_distance2d = 0;
+  for (const auto & llt : lanelet_sequence) {
+    const auto & centerline = llt.centerline();
+    for (auto it = centerline.begin(); std::next(it) != centerline.end(); ++it) {
+      const auto pt = *it;
+      const auto next_pt = *std::next(it);
+      const double distance2d = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt));
+      if (accumulated_distance2d + distance2d > s) {
+        const double ratio = (s - accumulated_distance2d) / distance2d;
+        const auto interpolated_pt = pt.basicPoint() * (1 - ratio) + next_pt.basicPoint() * ratio;
+        const auto yaw = std::atan2(next_pt.y() - pt.y(), next_pt.x() - pt.x());
+        Pose pose;
+        pose.position = createPoint(interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z());
+        pose.orientation = createQuaternionFromYaw(yaw);
+        return pose;
+      }
+      accumulated_distance2d += distance2d;
+    }
+  }
+  return Pose{};
+}
 }  // namespace autoware::route_handler

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

@@ -162,9 +162,17 @@ class NormalLaneChange : public LaneChangeBase
   bool hasEnoughLengthToTrafficLight(
     const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;
 
-  bool getLaneChangePaths(
-    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
-    Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const;
+  bool getLaneChangePaths(LaneChangePaths * candidate_paths) const;
+
+  LaneChangePath getCandidatePath(
+    const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics,
+    const PathWithLaneId & prep_segment, const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+    const Pose & lc_start_pose, const double target_lane_length, const double shift_length,
+    const double next_lc_buffer, const bool is_goal_in_route) const;
+
+  bool checkCandidatePathSafety(
+    const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects,
+    const double lane_change_buffer, const bool is_stuck) const;
 
   std::optional<LaneChangePath> calcTerminalLaneChangePath(
     const lanelet::ConstLanelets & current_lanes,

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

@@ -24,6 +24,7 @@ using autoware::route_handler::Direction;
 using autoware::route_handler::RouteHandler;
 using behavior_path_planner::lane_change::CommonDataPtr;
 using behavior_path_planner::lane_change::LCParamPtr;
+using behavior_path_planner::lane_change::PhaseMetrics;
 
 /**
  * @brief Calculates the distance from the ego vehicle to the terminal point.
@@ -132,6 +133,38 @@ double calc_maximum_lane_change_length(
 double calc_maximum_lane_change_length(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet,
   const double max_acc);
+
+/**
+ * @brief Calculates the distance required during a lane change operation.
+ *
+ * Used for computing prepare or lane change length based on current and maximum velocity,
+ * acceleration, and duration, returning the lesser of accelerated distance or distance at max
+ * velocity.
+ *
+ * @param velocity The current velocity of the vehicle in meters per second (m/s).
+ * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s).
+ * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2).
+ * @param duration The duration of the lane change in seconds (s).
+ * @return The calculated minimum distance in meters (m).
+ */
+double calc_phase_length(
+  const double velocity, const double maximum_velocity, const double acceleration,
+  const double duration);
+
+double calc_lane_changing_acceleration(
+  const double initial_lane_changing_velocity, const double max_path_velocity,
+  const double lane_changing_time, const double prepare_longitudinal_acc);
+
+std::vector<PhaseMetrics> calc_prepare_phase_metrics(
+  const CommonDataPtr & common_data_ptr, const std::vector<double> & prepare_durations,
+  const std::vector<double> & lon_accel_values, const double current_velocity,
+  const double min_length_threshold = 0.0,
+  const double max_length_threshold = std::numeric_limits<double>::max());
+
+std::vector<PhaseMetrics> calc_shift_phase_metrics(
+  const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity,
+  const double max_velocity, const double lon_accel,
+  const double max_length_threshold = std::numeric_limits<double>::max());
 }  // namespace autoware::behavior_path_planner::utils::lane_change::calculation
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_

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

@@ -192,6 +192,15 @@ struct PhaseInfo
   }
 };
 
+struct PhaseMetrics
+{
+  double duration{0.0};
+  double length{0.0};
+  double velocity{0.0};
+  double lon_accel{0.0};
+  double lat_accel{0.0};
+};
+
 struct Lanes
 {
   bool current_lane_in_goal_section{false};
@@ -215,6 +224,22 @@ struct Info
 
   double lateral_acceleration{0.0};
   double terminal_lane_changing_velocity{0.0};
+
+  Info() = default;
+  Info(
+    const PhaseMetrics & _prep_metrics, const PhaseMetrics & _lc_metrics,
+    const Pose & _lc_start_pose, const Pose & _lc_end_pose, const ShiftLine & _shift_line)
+  {
+    longitudinal_acceleration = PhaseInfo{_prep_metrics.lon_accel, _lc_metrics.lon_accel};
+    duration = PhaseInfo{_prep_metrics.duration, _lc_metrics.duration};
+    velocity = PhaseInfo{_prep_metrics.velocity, _prep_metrics.velocity};
+    length = PhaseInfo{_prep_metrics.length, _lc_metrics.length};
+    lane_changing_start = _lc_start_pose;
+    lane_changing_end = _lc_end_pose;
+    lateral_acceleration = _lc_metrics.lat_accel;
+    terminal_lane_changing_velocity = _lc_metrics.velocity;
+    shift_line = _shift_line;
+  }
 };
 
 template <typename Object>
@@ -316,6 +341,7 @@ using LaneChangeModuleType = lane_change::ModuleType;
 using LaneChangeParameters = lane_change::Parameters;
 using LaneChangeStates = lane_change::States;
 using LaneChangePhaseInfo = lane_change::PhaseInfo;
+using LaneChangePhaseMetrics = lane_change::PhaseMetrics;
 using LaneChangeInfo = lane_change::Info;
 using FilteredByLanesObjects = lane_change::LanesObjects<std::vector<PredictedObject>>;
 using FilteredByLanesExtendedObjects = lane_change::LanesObjects<ExtendedPredictedObjects>;

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

@@ -68,10 +68,6 @@ double calcMaximumAcceleration(
   const double current_velocity, const double current_max_velocity,
   const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters);
 
-double calcLaneChangingAcceleration(
-  const double initial_lane_changing_velocity, const double max_path_velocity,
-  const double lane_changing_time, const double prepare_longitudinal_acc);
-
 void setPrepareVelocity(
   PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity);
 
@@ -96,14 +92,9 @@ bool isPathInLanelets(
 
 std::optional<LaneChangePath> constructCandidatePath(
   const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info,
-  const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
-  const PathWithLaneId & target_lane_reference_path,
+  const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path,
   const std::vector<std::vector<int64_t>> & sorted_lane_ids);
 
-ShiftLine getLaneChangingShiftLine(
-  const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
-  const PathWithLaneId & reference_path, const double shift_length);
-
 ShiftLine getLaneChangingShiftLine(
   const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose,
   const PathWithLaneId & reference_path, const double shift_length);
@@ -254,23 +245,6 @@ bool isWithinIntersection(
  */
 bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon);
 
-/**
- * @brief Calculates the distance required during a lane change operation.
- *
- * Used for computing prepare or lane change length based on current and maximum velocity,
- * acceleration, and duration, returning the lesser of accelerated distance or distance at max
- * velocity.
- *
- * @param velocity The current velocity of the vehicle in meters per second (m/s).
- * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s).
- * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2).
- * @param duration The duration of the lane change in seconds (s).
- * @return The calculated minimum distance in meters (m).
- */
-double calcPhaseLength(
-  const double velocity, const double maximum_velocity, const double acceleration,
-  const double time);
-
 LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr);
 
 bool is_same_lane_with_prev_iteration(