Merge pull request #1735 from tier4/ref/transient

fix: cherry-pick autowarefoundation#8997 and autowarefoundation#9279

Author: shmpwk

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

@@ -142,9 +142,8 @@ class NormalLaneChange : public LaneChangeBase
   FilteredByLanesObjects filterObjectsByLanelets(
     const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const;
 
-  PathWithLaneId getPrepareSegment(
-    const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
-    const double prepare_length) const override;
+  bool get_prepare_segment(
+    PathWithLaneId & prepare_segment, const double prepare_length) const override;
 
   PathWithLaneId getTargetSegment(
     const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose,
@@ -155,17 +154,21 @@ class NormalLaneChange : public LaneChangeBase
     const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const;
 
+  std::vector<LaneChangePhaseMetrics> get_prepare_metrics() const;
+  std::vector<LaneChangePhaseMetrics> get_lane_changing_metrics(
+    const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics,
+    const double shift_length, const double dist_to_reg_element) const;
+
   bool get_lane_change_paths(LaneChangePaths & candidate_paths) 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;
+    const Pose & lc_start_pose, const double shift_length) 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;
+    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/base_class.hpp

@@ -238,9 +238,8 @@ class LaneChangeBase
 protected:
   virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
 
-  virtual PathWithLaneId getPrepareSegment(
-    const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
-    const double prepare_length) const = 0;
+  virtual bool get_prepare_segment(
+    PathWithLaneId & prepare_segment, const double prepare_length) const = 0;
 
   virtual bool isValidPath(const PathWithLaneId & path) const = 0;
 

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

@@ -338,6 +338,11 @@ struct TransientData
   double max_prepare_length{
     std::numeric_limits<double>::max()};  // maximum prepare length, starting from ego's base link
 
+  double target_lane_length{std::numeric_limits<double>::min()};
+
+  lanelet::ArcCoordinates current_lanes_ego_arc;  // arc coordinates of ego pose along current lanes
+  lanelet::ArcCoordinates target_lanes_ego_arc;   // arc coordinates of ego pose along target lanes
+
   bool is_ego_near_current_terminal_start{false};
 };
 

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

@@ -79,9 +79,7 @@ std::vector<int64_t> replaceWithSortedIds(
   const std::vector<int64_t> & original_lane_ids,
   const std::vector<std::vector<int64_t>> & sorted_lane_ids);
 
-std::vector<std::vector<int64_t>> getSortedLaneIds(
-  const RouteHandler & route_handler, const Pose & current_pose,
-  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes);
+std::vector<std::vector<int64_t>> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr);
 
 lanelet::ConstLanelets getTargetNeighborLanes(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
@@ -104,12 +102,9 @@ 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);
 
-PathWithLaneId getReferencePathFromTargetLane(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
-  const Pose & lane_changing_start_pose, const double target_lane_length,
-  const double lane_changing_length, const double forward_path_length,
-  const double resample_interval, const bool is_goal_in_route,
-  const double next_lane_change_buffer);
+PathWithLaneId get_reference_path_from_target_Lane(
+  const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose,
+  const double lane_changing_length, const double resample_interval);
 
 std::vector<DrivableLanes> generateDrivableLanes(
   const std::vector<DrivableLanes> & original_drivable_lanes, const RouteHandler & route_handler,
@@ -145,8 +140,7 @@ bool isParkedObject(
 
 bool passed_parked_objects(
   const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
-  const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
-  CollisionCheckDebugMap & object_debug);
+  const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & object_debug);
 
 std::optional<size_t> getLeadingStaticObjectIdx(
   const RouteHandler & route_handler, const LaneChangePath & lane_change_path,