update

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

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

@@ -59,9 +59,9 @@ PathWithLaneId resamplePathWithSpline(
   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);
+  const std::vector<PathPointWithLaneId> & following_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, const double stopping_time = 0.0);
 
 Path toPath(const PathWithLaneId & input);
 

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

@@ -16,8 +16,8 @@
 
 #include "behavior_path_planner/utils/create_vehicle_footprint.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
-#include "behavior_path_planner/utils/start_planner/util.hpp"
 #include "behavior_path_planner/utils/safety_check.hpp"
+#include "behavior_path_planner/utils/start_planner/util.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -829,14 +829,14 @@ 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);
+  const auto & ego_predicted_path = utils::createPredictedPathFromTargetVelocity(
+    path, current_twist, current_pose, current_seg_idx, check_end_time, time_resolution,
+    prepare_duration, prepare_acc, lane_changing_acc);
   return utils::safety_check::isSafeInLaneletCollisionCheck(
-            pull_out_path, interpolated_ego, current_twist, check_durations,
-            lane_change_path.duration.prepare, obj, obj_path, common_parameter,
-            lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel,
-            rear_decel, current_debug_data.second);
+    pull_out_path, interpolated_ego, current_twist, check_durations,
+    lane_change_path.duration.prepare, obj, obj_path, common_parameter,
+    lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel, rear_decel,
+    current_debug_data.second);
 }
 
 void StartPlannerModule::setDebugData() const

planning/behavior_path_planner/src/utils/path_utils.cpp

@@ -127,9 +127,10 @@ PathWithLaneId resamplePathWithSpline(
 }
 
 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)
+  const std::vector<PathPointWithLaneId> & following_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, const double stopping_time)
 {
   PredictedPath predicted_path{};
   predicted_path.time_step = rclcpp::Duration::from_seconds(resolution);
@@ -142,23 +143,41 @@ PredictedPath createPredictedPathFromTargetVelocity(
   FrenetPoint vehicle_pose_frenet =
     convertToFrenetPoint(trajectory.points, pose.position, nearest_seg_idx);
 
+  // add a segment to the path
+  auto addSegment = [&](
+                      std::vector<Pose> & path, const double initial_velocity, const double acc,
+                      const double start_time, const double end_time, const double offset) {
+    for (double t = start_time; t < end_time; t += resolution) {
+      const double delta_t = t - start_time;
+      const double length = initial_velocity * delta_t + 0.5 * acc * delta_t * delta_t + offset;
+      path.push_back(motion_utils::calcInterpolatedPose(path, vehicle_pose_frenet.length + length));
+    }
+  };
+
   // 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;
   }
 
+  // Stopping segment, only if stopping_time is greater than zero
+  if(stopping_time > 0.0)
+  {
+    addSegment(predicted_path.path, 0.0, 0.0, 0.0, stopping_time, 0.0);
+  }
+
   // Acceleration segment
+  double offset = 0.0;
   addSegment(
-    predicted_path.path, current_velocity, acc_till_target_velocity, 0.0, acc_time, resolution,
-    vehicle_pose_frenet);
+    predicted_path.path, current_velocity, acc_till_target_velocity, stopping_time,
+    stopping_time + acc_time, offset);
 
   // Constant velocity segment
-  const double offset =
-    current_velocity * acc_time + 0.5 * acc_till_target_velocity * acc_time * acc_time;
+  offset += current_velocity * acc_time + 0.5 * acc_till_target_velocity * acc_time * acc_time;
+  double constant_velocity_time = stopping_time + acc_time + (target_velocity / resolution);
   addSegment(
-    predicted_path.path, target_velocity, 0.0, acc_time, acc_time + (target_velocity / resolution),
-    resolution, vehicle_pose_frenet, offset);
+    predicted_path.path, target_velocity, 0.0, stopping_time + acc_time, constant_velocity_time,
+    offset);
 
   return predicted_path;
 }