update

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

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

@@ -60,7 +60,7 @@ PathWithLaneId resamplePathWithSpline(
   const std::pair<double, double> target_section = {0.0, std::numeric_limits<double>::max()});
 
 PredictedPath createPredictedPathFromTargetVelocity(
-  const std::vector<PathPointWithLaneId> & following_trajectory, const double current_velocity,
+  const PathWithLaneId & 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);
 

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

@@ -824,15 +824,13 @@ void StartPlannerModule::setDebugData() const
   const auto add = [this](const MarkerArray & added) {
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
   };
+  size_t nearest_segment_index = motion_utils::findNearestIndex(
+    status_.pull_out_path.partial_paths.back().points,
+    planner_data_->self_odometry->pose.pose.position);
 
   const auto & ego_predicted_path = utils::createPredictedPathFromTargetVelocity(
-    status_.pull_out_path.partial_paths.back().points,
-    planner_data_->self_odometry->twist.twist.linear.x, 2.0, 0.5,
-    planner_data_->self_odometry->pose.pose,
-    motion_utils::findNearestIndex(
-      status_.pull_out_path.partial_paths.back().points,
-      planner_data_->self_odometry->pose.pose.position),
-    0.5, 1.0);
+    status_.pull_out_path.partial_paths.back(), planner_data_->self_odometry->twist.twist.linear.x,
+    2.0, 0.5, planner_data_->self_odometry->pose.pose, nearest_segment_index, 0.5, 1.0);
 
   debug_marker_.markers.clear();
   add(createEgoPredictedPathMarkerArray(

planning/behavior_path_planner/src/utils/path_utils.cpp

@@ -127,33 +127,34 @@ PathWithLaneId resamplePathWithSpline(
 }
 
 PredictedPath createPredictedPathFromTargetVelocity(
-  const std::vector<PathPointWithLaneId> & following_trajectory, const double current_velocity,
+  const PathWithLaneId & 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);
-  predicted_path.path.reserve(std::min(following_trajectory.size(), static_cast<size_t>(100)));
+  predicted_path.path.reserve(
+    std::min(following_trajectory.points.size(), static_cast<size_t>(100)));
 
-  if (following_trajectory.empty()) {
+  if (following_trajectory.points.empty()) {
+    std::cerr << "Trajectory path is empty" << std::endl;
     return predicted_path;
   }
 
   FrenetPoint vehicle_pose_frenet =
-    convertToFrenetPoint(following_trajectory, pose.position, nearest_seg_idx);
+    convertToFrenetPoint(following_trajectory.points, pose.position, nearest_seg_idx);
 
   // add a segment to the path
   auto addSegment = [&](
-                      std::vector<PathPointWithLaneId> & following_trajectory,
+                      const std::vector<PathPointWithLaneId> & following_trajectory_points,
                       PredictedPath & predicted_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;
       const auto pose = motion_utils::calcInterpolatedPose(
-        following_trajectory, vehicle_pose_frenet.length + length);
-      // std::cerr << "pose: " << pose.position.x << ", " << pose.position.y << std::endl;
+        following_trajectory_points, vehicle_pose_frenet.length + length);
       predicted_path.path.push_back(pose);
     }
   };
@@ -166,21 +167,20 @@ PredictedPath createPredictedPathFromTargetVelocity(
 
   // Stopping segment, only if stopping_time is greater than zero
   if (stopping_time > 0.0) {
-    addSegment(
-      following_trajectory, predicted_path, 0.0, 0.0, 0.0, static_cast<double>(stopping_time), 0.0);
+    addSegment(following_trajectory.points, predicted_path, 0.0, 0.0, 0.0, stopping_time, 0.0);
   }
 
   // Acceleration segment
   double offset = 0.0;
   addSegment(
-    following_trajectory, predicted_path, current_velocity, acc_till_target_velocity, stopping_time,
-    stopping_time + acc_time, offset);
+    following_trajectory.points, predicted_path, current_velocity, acc_till_target_velocity,
+    stopping_time, stopping_time + acc_time, offset);
 
   // Constant velocity segment
   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(
-    following_trajectory, predicted_path, target_velocity, 0.0, stopping_time + acc_time,
+    following_trajectory.points, predicted_path, target_velocity, 0.0, stopping_time + acc_time,
     constant_velocity_time, offset);
 
   return predicted_path;