refactor(lane_change): refactor getLaneChangePaths function (#8909)

* refactor lane change utility funcions

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

* LC utility function to get distance to next regulatory element

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

* don't activate LC module when close to regulatory element

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

* modify threshold distance calculation

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

* move regulatory element check to canTransitFailureState() function

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

* always run LC module if approaching terminal point

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

* use max possible LC length as threshold

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

* update LC readme

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

* refactor implementation

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

* update readme

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

* refactor checking data validity

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

* refactor sampling of prepare phase metrics and lane changing phase metrics

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

* add route handler function to get pose from 2d arc length

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

* refactor candidate path generation

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

* refactor candidate path safety check

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

* fix variable name

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

* Update planning/autoware_route_handler/src/route_handler.cpp

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

* correct parameter name

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

* set prepare segment velocity after taking max path velocity value

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

* update LC README

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

* minor changes

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

* check phase length difference with previos valid candidate path

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

* change logger name

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

* change functions names to snake case

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

* use snake case for function names

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

* add colors to flow chart in README

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

---------

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

Author: mkquda

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 get_pose_from_2d_arc_length(
+    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

@@ -47,6 +47,8 @@ namespace autoware::route_handler
 {
 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 @@ std::optional<lanelet::routing::LaneletPath> RouteHandler::findDrivableLanePath(
   if (route) return route->shortestPath();
   return {};
 }
+
+Pose RouteHandler::get_pose_from_2d_arc_length(
+  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/README.md

@@ -24,6 +24,108 @@ The lane change candidate path is divided into two phases: preparation and lane-
 
 ![lane-change-phases](./images/lane_change-lane_change_phases.png)
 
+The following chart illustrates the process of sampling candidate paths for lane change.
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #WHITE
+
+start
+
+if (Is current lanes, target lanes OR target neighbor lanes polygon empty?) then (yes)
+  #LightPink:Return prev approved path;
+  stop
+else (no)
+endif
+
+:Get target objects;
+
+:Calculate prepare phase metrics;
+
+:Start loop through prepare phase metrics;
+
+repeat
+  :Get prepare segment;
+
+  if (Is LC start point outside target lanes range) then (yes)
+    if (Is found a valid path) then (yes)
+      #Orange:Return first valid path;
+      stop
+    else (no)
+      #LightPink:Return prev approved path;
+      stop
+    endif
+  else (no)
+  endif
+
+  :Calculate shift phase metrics;
+
+  :Start loop through shift phase metrics;
+  repeat
+    :Get candidate path;
+    note left: Details shown in the next chart
+    if (Is valid candidate path?) then (yes)
+      :Store candidate path;
+      if (Is candidate path safe?) then (yes)
+        #LightGreen:Return candidate path;
+        stop
+      else (no)
+      endif
+    else (no)
+    endif
+    repeat while (Finished looping shift phase metrics) is (FALSE)
+  repeat while (Is finished looping prepare phase metrics) is (FALSE)
+
+if (Is found a valid path) then (yes)
+  #Orange:Return first valid path;
+  stop
+else (no)
+endif
+
+#LightPink:Return prev approved path;
+stop
+
+@enduml
+```
+
+While the following chart demonstrates the process of generating a valid candidate path.
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #WHITE
+
+start
+
+:Calculate resample interval;
+
+:Get reference path from target lanes;
+
+if (Is reference path empty?) then (yes)
+  #LightPink:Return;
+  stop
+else (no)
+endif
+
+:Get LC shift line;
+
+:Set lane change Info;
+note left: set information from sampled prepare phase and shift phase metrics\n<color:red>(duration, length, velocity, and acceleration)
+
+:Construct candidate path;
+
+if (Candidate path has enough length?) then (yes)
+  #LightGreen:Return valid candidate path;
+  stop
+else (no)
+  #LightPink:Return;
+  stop
+endif
+
+@enduml
+```
+
 ### Preparation phase
 
 The preparation trajectory is the candidate path's first and the straight portion generated along the ego vehicle's current lane. The length of the preparation trajectory is computed as follows.

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

@@ -153,18 +153,17 @@ class NormalLaneChange : public LaneChangeBase
     const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const;
 
-  bool hasEnoughLengthToCrosswalk(
-    const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;
+  bool get_lane_change_paths(LaneChangePaths & candidate_paths) const;
 
-  bool hasEnoughLengthToIntersection(
-    const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;
+  LaneChangePath get_candidate_path(
+    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 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 check_candidate_path_safety(
+    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

@@ -193,6 +193,28 @@ struct PhaseInfo
   }
 };
 
+struct PhaseMetrics
+{
+  double duration{0.0};
+  double length{0.0};
+  double velocity{0.0};
+  double sampled_lon_accel{0.0};
+  double actual_lon_accel{0.0};
+  double lat_accel{0.0};
+
+  PhaseMetrics(
+    const double _duration, const double _length, const double _velocity,
+    const double _sampled_lon_accel, const double _actual_lon_accel, const double _lat_accel)
+  : duration(_duration),
+    length(_length),
+    velocity(_velocity),
+    sampled_lon_accel(_sampled_lon_accel),
+    actual_lon_accel(_actual_lon_accel),
+    lat_accel(_lat_accel)
+  {
+  }
+};
+
 struct Lanes
 {
   bool current_lane_in_goal_section{false};
@@ -216,6 +238,23 @@ 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.actual_lon_accel, _lc_metrics.actual_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>
@@ -317,6 +356,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

@@ -69,10 +69,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);
 
@@ -99,17 +95,12 @@ bool pathFootprintExceedsTargetLaneBound(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
   const double margin = 0.1);
 
-std::optional<LaneChangePath> constructCandidatePath(
+std::optional<LaneChangePath> construct_candidate_path(
   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(
+ShiftLine get_lane_changing_shift_line(
   const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose,
   const PathWithLaneId & reference_path, const double shift_length);
 
@@ -260,23 +251,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(