temo

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp

@@ -58,6 +58,11 @@ PathWithLaneId resamplePathWithSpline(
   const PathWithLaneId & path, const double interval, const bool keep_input_points = false,
   const std::pair<double, double> target_section = {0.0, std::numeric_limits<double>::max()});
 
+const PredictedPath createPredictedPathFromTargetVelocity(
+  const PathWithLaneId & trajectory, const double & current_velocity,
+  const double & target_velocity, const double acc_till_target_velocity, const Pose & pose,
+  const size_t nearest_seg_idx, const double resolution);
+
 Path toPath(const PathWithLaneId & input);
 
 size_t getIdxByArclength(

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -828,6 +828,7 @@ bool StartPlannerModule::isSafeConsideringDynamicObjects()
 {
   // TODO(Sugahara): should safety check for backward path later
   const auto & pull_out_path = status_.pull_out_path.partial_paths.at(0);
+  // create ego predicted path
   const auto & ego_predicted_path = convertToPredictedPath(
   path, current_twist, current_pose, current_seg_idx, check_end_time, time_resolution,
   prepare_duration, prepare_acc, lane_changing_acc);

planning/behavior_path_planner/src/utils/path_utils.cpp

@@ -126,6 +126,43 @@ PathWithLaneId resamplePathWithSpline(
   return motion_utils::resamplePath(path, s_out);
 }
 
+PredictedPath createPredictedPathFromTargetVelocity(
+  const PathWithLaneId & trajectory, const double & current_velocity,
+  const double & target_velocity, const double acc_till_target_velocity, const Pose & pose,
+  const size_t nearest_seg_idx, const double resolution)
+{
+  PredictedPath predicted_path{};
+  predicted_path.time_step = rclcpp::Duration::from_seconds(resolution);
+  predicted_path.path.reserve(std::min(trajectory.points.size(), static_cast<size_t>(100)));
+
+  if (trajectory.points.empty()) {
+    return predicted_path;
+  }
+
+  FrenetPoint vehicle_pose_frenet =
+    convertToFrenetPoint(trajectory.points, pose.position, nearest_seg_idx);
+
+  // Calculate time required to reach target velocity
+  double acc_time = (target_velocity - current_velocity) / acc_till_target_velocity;
+  if (acc_time < 0.0) {
+    acc_time = 0.0;
+  }
+
+  // Acceleration segment
+  addSegment(
+    predicted_path.path, current_velocity, acc_till_target_velocity, 0.0, acc_time, resolution,
+    vehicle_pose_frenet);
+
+  // Constant velocity segment
+  const double offset =
+    current_velocity * acc_time + 0.5 * acc_till_target_velocity * acc_time * acc_time;
+  addSegment(
+    predicted_path.path, target_velocity, 0.0, acc_time, acc_time + (target_velocity / resolution),
+    resolution, vehicle_pose_frenet, offset);
+
+  return predicted_path;
+}
+
 Path toPath(const PathWithLaneId & input)
 {
   Path output{};