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

* refactor lane change utility funcions Signed-off-by: mohammad alqudah <[email protected]> * LC utility function to get distance to next regulatory element Signed-off-by: mohammad alqudah <[email protected]> * don't activate LC module when close to regulatory element Signed-off-by: mohammad alqudah <[email protected]> * modify threshold distance calculation Signed-off-by: mohammad alqudah <[email protected]> * move regulatory element check to canTransitFailureState() function Signed-off-by: mohammad alqudah <[email protected]> * always run LC module if approaching terminal point Signed-off-by: mohammad alqudah <[email protected]> * use max possible LC length as threshold Signed-off-by: mohammad alqudah <[email protected]> * update LC readme Signed-off-by: mohammad alqudah <[email protected]> * refactor implementation Signed-off-by: mohammad alqudah <[email protected]> * update readme Signed-off-by: mohammad alqudah <[email protected]> * refactor checking data validity Signed-off-by: mohammad alqudah <[email protected]> * refactor sampling of prepare phase metrics and lane changing phase metrics Signed-off-by: mohammad alqudah <[email protected]> * add route handler function to get pose from 2d arc length Signed-off-by: mohammad alqudah <[email protected]> * refactor candidate path generation Signed-off-by: mohammad alqudah <[email protected]> * refactor candidate path safety check Signed-off-by: mohammad alqudah <[email protected]> * fix variable name Signed-off-by: mohammad alqudah <[email protected]> * Update planning/autoware_route_handler/src/route_handler.cpp Co-authored-by: Zulfaqar Azmi <[email protected]> * correct parameter name Signed-off-by: mohammad alqudah <[email protected]> * set prepare segment velocity after taking max path velocity value Signed-off-by: mohammad alqudah <[email protected]> * update LC README Signed-off-by: mohammad alqudah <[email protected]> * minor changes Signed-off-by: mohammad alqudah <[email protected]> * check phase length difference with previos valid candidate path Signed-off-by: mohammad alqudah <[email protected]> * change logger name Signed-off-by: mohammad alqudah <[email protected]> * change functions names to snake case Signed-off-by: mohammad alqudah <[email protected]> * use snake case for function names Signed-off-by: mohammad alqudah <[email protected]> * add colors to flow chart in README Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Co-authored-by: Zulfaqar Azmi <[email protected]>
autowarefoundation · Sep 25, 2024 · 656a78e · 656a78e
1 parent 73a9788
commit 656a78e
Show file tree

Hide file tree

Showing 10 changed files with 559 additions and 396 deletions.
diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/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_;

diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/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
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
@@ -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.

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
@@ -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,

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
@@ -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_
diff --git a/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/...ne_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>;

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