refactor(lane_change): separate path-related function to utils/path

Signed-off-by: Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt

@@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/utils/calculation.cpp
   src/utils/markers.cpp
   src/utils/utils.cpp
+  src/utils/path.cpp
 )
 
 if(BUILD_TESTING)

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

@@ -235,9 +235,6 @@ class LaneChangeBase
 protected:
   virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
 
-  virtual bool get_prepare_segment(
-    PathWithLaneId & prepare_segment, const double prepare_length) const = 0;
-
   virtual bool isValidPath(const PathWithLaneId & path) const = 0;
 
   virtual bool isAbleToStopSafely() const = 0;

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

@@ -128,45 +128,24 @@ class NormalLaneChange : public LaneChangeBase
 
   void filterOncomingObjects(PredictedObjects & objects) const;
 
-  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,
-    const double target_lane_length, const double lane_changing_length,
-    const double lane_changing_velocity, const double buffer_for_next_lane_change) 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 shift_length) const;
-
   bool check_candidate_path_safety(
     const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const;
 
-  std::optional<LaneChangePath> calcTerminalLaneChangePath(
-    const lanelet::ConstLanelets & current_lanes,
-    const lanelet::ConstLanelets & target_lanes) const;
-
   bool isValidPath(const PathWithLaneId & path) const override;
 
   PathSafetyStatus isLaneChangePathSafe(
     const LaneChangePath & lane_change_path,
+    const std::vector<std::vector<PoseWithVelocityStamped>> & ego_predicted_paths,
     const lane_change::TargetObjects & collision_check_objects,
     const utils::path_safety_checker::RSSparams & rss_params,
-    const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const;
-
-  bool has_collision_with_decel_patterns(
-    const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects,
-    const size_t deceleration_sampling_num, const RSSparams & rss_param,
-    const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const;
+    CollisionCheckDebugMap & debug_data) const;
 
   bool is_collided(
     const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj,
@@ -178,24 +157,6 @@ class NormalLaneChange : public LaneChangeBase
 
   bool is_ego_stuck() const;
 
-  /**
-   * @brief Checks if the given pose is a valid starting point for a lane change.
-   *
-   * This function determines whether the given pose (position) of the vehicle is within
-   * the area of either the target neighbor lane or the target lane itself. It uses geometric
-   * checks to see if the start point of the lane change is covered by the polygons representing
-   * these lanes.
-   *
-   * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
-   *  - Non-null `lanes_polygon_ptr` that contains the polygon data for lanes.
-   * @param pose The current pose of the vehicle
-   *
-   * @return `true` if the pose is within either the target neighbor lane or the target lane;
-   * `false` otherwise.
-   */
-  bool is_valid_start_point(
-    const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const;
-
   bool check_prepare_phase() const;
 
   void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path);
@@ -213,7 +174,6 @@ class NormalLaneChange : public LaneChangeBase
 
   std::vector<PathPointWithLaneId> path_after_intersection_;
   double stop_time_{0.0};
-  static constexpr double floating_err_th{1e-3};
 };
 }  // namespace autoware::behavior_path_planner
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp

@@ -160,6 +160,8 @@ struct TargetObjects
   : leading(std::move(leading)), trailing(std::move(trailing))
   {
   }
+
+  [[nodiscard]] bool empty() const { return leading.empty() && trailing.empty(); }
 };
 
 enum class ModuleType {

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

@@ -0,0 +1,48 @@
+// Copyright 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// 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_
+
+#include "autoware/behavior_path_lane_change_module/structs/data.hpp"
+#include "autoware/behavior_path_lane_change_module/structs/path.hpp"
+
+#include <autoware/behavior_path_planner_common/utils/utils.hpp>
+
+#include <vector>
+
+namespace autoware::behavior_path_planner::utils::lane_change
+{
+using behavior_path_planner::LaneChangePath;
+using behavior_path_planner::lane_change::CommonDataPtr;
+
+bool get_prepare_segment(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path,
+  const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment);
+
+LaneChangePath get_candidate_path(
+  const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric,
+  const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids, const Pose & lc_start_pose,
+  const double shift_length);
+
+std::optional<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);
+
+std::optional<LaneChangePath> calcTerminalLaneChangePath(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path);
+}  // 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

@@ -52,6 +52,7 @@ using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_perception_msgs::msg::PredictedPath;
 using behavior_path_planner::lane_change::CommonDataPtr;
 using behavior_path_planner::lane_change::LanesPolygon;
+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;
@@ -60,15 +61,9 @@ using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using path_safety_checker::CollisionCheckDebugMap;
 using tier4_planning_msgs::msg::PathWithLaneId;
-
+rclcpp::Logger get_logger();
 bool is_mandatory_lane_change(const ModuleType lc_type);
 
-double calcLaneChangeResampleInterval(
-  const double lane_changing_length, const double lane_changing_velocity);
-
-void setPrepareVelocity(
-  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);
@@ -79,27 +74,10 @@ lanelet::ConstLanelets get_target_neighbor_lanes(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
   const LaneChangeModuleType & type);
 
-bool isPathInLanelets(
-  const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes);
-
 bool path_footprint_exceeds_target_lane_bound(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
   const double margin = 0.1);
 
-std::optional<LaneChangePath> construct_candidate_path(
-  const CommonDataPtr & common_data_ptr, 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);
-
-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 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,
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes);
@@ -141,8 +119,8 @@ bool isParkedObject(
  * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects
  * which pass isParkedObject() check will be considered.
  *
- * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters,
- *                        and transient data.
+ * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info,
+ * parameters, and transient data.
  * @param lane_change_path Candidate lane change path to apply checks on.
  * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include
  *                       target lane leading static objects).
@@ -202,8 +180,8 @@ rclcpp::Logger getLogger(const std::string & type);
  * The footprint is determined by the vehicle's pose and its dimensions, including the distance
  * from the base to the front and rear ends of the vehicle, as well as its width.
  *
- * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions
- *                        and pose information.
+ * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's
+ * dimensions and pose information.
  *
  * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle.
  */
@@ -233,15 +211,15 @@ bool is_within_intersection(
 /**
  * @brief Determines if a polygon is within lanes designated for turning.
  *
- * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight').
- * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's
- * area.
+ * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding
+ * 'straight'). It evaluates the lanelet's 'turn_direction' attribute and determines overlap with
+ * the lanelet's area.
  *
  * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated.
  * @param polygon The polygon to be checked for its presence within turn direction lanes.
  *
- * @return bool True if the polygon is within a lane designated for turning, false if it is within a
- *              straight lane or no turn direction is specified.
+ * @return bool True if the polygon is within a lane designated for turning, false if it is within
+ * a straight lane or no turn direction is specified.
  */
 bool is_within_turn_direction_lanes(
   const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon);
@@ -276,8 +254,8 @@ double get_distance_to_next_regulatory_element(
  *
  * @param common_data_ptr Pointer to the common data structure containing parameters for lane
  * change.
- * @param filtered_objects A collection of objects filtered by lanes, including those in the current
- * lane.
+ * @param filtered_objects A collection of objects filtered by lanes, including those in the
+ * current lane.
  * @param dist_to_target_lane_start The distance to the start of the target lane from the ego
  * vehicle.
  * @param path The current path of the ego vehicle, containing path points and lane information.
@@ -297,8 +275,8 @@ double get_min_dist_to_current_lanes_obj(
  *
  * @param common_data_ptr Pointer to the common data structure containing parameters for the lane
  * change.
- * @param filtered_objects A collection of objects filtered by lanes, including those in the target
- * lane.
+ * @param filtered_objects A collection of objects filtered by lanes, including those in the
+ * target lane.
  * @param stop_arc_length The arc length at which the ego vehicle is expected to stop.
  * @param path The current path of the ego vehicle, containing path points and lane information.
  * @return true if there is an object in the target lane that influences the stop point decision;
@@ -365,14 +343,15 @@ bool has_overtaking_turn_lane_object(
  *
  * @param common_data_ptr Shared pointer to CommonData containing information about current lanes,
  *                        vehicle dimensions, lane polygons, and behavior parameters.
- * @param object An extended predicted object representing a potential obstacle in the environment.
+ * @param object An extended predicted object representing a potential obstacle in the
+ * environment.
  * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the
  * current lanes.
  * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle.
- * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of
- * the lane.
- * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or
- * outside boundaries).
+ * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point
+ * of the lane.
+ * @param leading_objects Reference to a structure for storing leading objects (stopped, moving,
+ * or outside boundaries).
  * @param trailing_objects Reference to a collection for storing trailing objects.
  *
  * @return true if the object is classified as either leading or trailing, false otherwise.
@@ -415,5 +394,11 @@ std::vector<lanelet::ConstLanelets> get_preceding_lanes(const CommonDataPtr & co
  */
 bool object_path_overlaps_lanes(
   const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon);
+
+std::vector<std::vector<PoseWithVelocityStamped>> convert_to_predicted_paths(
+  const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
+  const size_t deceleration_sampling_num);
+
+bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_