frenet planner

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

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

@@ -250,6 +250,7 @@ struct CommonData
   LanesPolygonPtr lanes_polygon_ptr;
   TransientData transient_data;
   PathWithLaneId current_lanes_path;
+  PathWithLaneId target_lanes_path;
   ModuleType lc_type;
   Direction direction;
 

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

@@ -19,27 +19,57 @@
 #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
+#include <autoware_frenet_planner/structures.hpp>
+
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <utility>
 #include <vector>
 
 namespace autoware::behavior_path_planner::lane_change
 {
+enum class PathType { ConstantJerk = 0, FrenetPlanner };
+
 using autoware::behavior_path_planner::TurnSignalInfo;
 using tier4_planning_msgs::msg::PathWithLaneId;
+struct TrajectoryGroup
+{
+  PathWithLaneId prepare;
+  PathWithLaneId target_lane_ref_path;
+  std::vector<double> target_lane_ref_path_dist;
+  LaneChangePhaseMetrics prepare_metric;
+  frenet_planner::Trajectory lane_changing;
+  frenet_planner::FrenetState initial_state;
+
+  TrajectoryGroup() = default;
+  TrajectoryGroup(
+    PathWithLaneId prepare, PathWithLaneId target_lane_ref_path,
+    std::vector<double> target_lane_ref_path_dist, LaneChangePhaseMetrics prepare_metric,
+    frenet_planner::Trajectory lane_changing, frenet_planner::FrenetState initial_state)
+  : prepare(std::move(prepare)),
+    target_lane_ref_path(std::move(target_lane_ref_path)),
+    target_lane_ref_path_dist(std::move(target_lane_ref_path_dist)),
+    prepare_metric(prepare_metric),
+    lane_changing(std::move(lane_changing)),
+    initial_state(initial_state)
+  {
+  }
+};
+
 struct Path
 {
+  Info info;
   PathWithLaneId path;
   ShiftedPath shifted_path;
-  Info info;
+  TrajectoryGroup frenet_path;
+  PathType type = PathType::ConstantJerk;
 };
 
 struct Status
 {
   Path lane_change_path;
   bool is_safe{false};
   bool is_valid_path{false};
-  double start_distance{0.0};
 };
 }  // namespace autoware::behavior_path_planner::lane_change
 

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

@@ -12,9 +12,6 @@
 // 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"
 
@@ -26,6 +23,7 @@ namespace autoware::behavior_path_planner::utils::lane_change
 {
 using behavior_path_planner::LaneChangePath;
 using behavior_path_planner::lane_change::CommonDataPtr;
+using behavior_path_planner::lane_change::TrajectoryGroup;
 
 /**
  * @brief Generates a prepare segment for a lane change maneuver.
@@ -98,5 +96,11 @@ 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);
+
+/// @brief generate lane change candidate paths using the Frenet planner
+std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path,
+  const std::vector<LaneChangePhaseMetrics> & metrics);
+
+std::optional<LaneChangePath> get_candidate_path(const TrajectoryGroup & trajectory_group);
 }  // 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

@@ -24,6 +24,8 @@
 
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware_frenet_planner/structures.hpp>
+#include <autoware_sampler_common/transform/spline_transform.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -56,6 +58,7 @@ 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;
+using behavior_path_planner::lane_change::TrajectoryGroup;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
@@ -83,6 +86,10 @@ bool path_footprint_exceeds_target_lane_bound(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
   const double margin = 0.1);
 
+void filter_out_of_bound_trajectories(
+  const CommonDataPtr & common_data_ptr,
+  std::vector<lane_change::TrajectoryGroup> & trajectory_groups);
+
 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);
@@ -432,5 +439,11 @@ std::vector<std::vector<PoseWithVelocityStamped>> convert_to_predicted_paths(
  * @return true if the pose is within the target or target neighbor polygons, false otherwise.
  */
 bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose);
+
+std::vector<PoseWithVelocityStamped> convert_to_predicted_path(
+  const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate,
+  [[maybe_unused]] const size_t deceleration_sampling_num);
+
+double calc_limit(const CommonDataPtr & common_data_ptr, const Pose & lc_end_pose);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml

@@ -23,6 +23,7 @@
 
   <depend>autoware_behavior_path_planner</depend>
   <depend>autoware_behavior_path_planner_common</depend>
+  <depend>autoware_frenet_planner</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_rtc_interface</depend>
   <depend>autoware_universe_utils</depend>