refactoring

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

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

@@ -59,12 +59,25 @@ 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()});
 
-// TODO(Sugahara): Add explanation and unit test
+/**
+ * @brief Create a predicted path from the current velocity to a target velocity
+ * @param [in] following_trajectory_points The trajectory points that the vehicle is supposed to
+ * follow
+ * @param [in] current_velocity The current velocity of the vehicle
+ * @param [in] target_velocity The desired velocity the vehicle should reach
+ * @param [in] acc_till_target_velocity The acceleration of the vehicle until it reaches the target
+ * velocity(constant)
+ * @param [in] pose The current pose of the vehicle, including its position and orientation
+ * @param [in] resolution The time resolution for the path prediction, affecting the granularity of
+ * the generated path
+ * @param [in] stopping_time The time required for starting to run
+ * @return An object of type PredictedPath that contains the predicted path of the vehicle
+ */
 PredictedPath createPredictedPathFromTargetVelocity(
   const std::vector<PathPointWithLaneId> & following_trajectory_points,
   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);
+  const double acc_till_target_velocity, const Pose & pose, const double resolution,
+  const double stopping_time);
 
 Path toPath(const PathWithLaneId & input);
 

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

@@ -824,19 +824,17 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
 
 void StartPlannerModule::setDebugData() const
 {
-  using marker_utils::createEgoPredictedPathMarkerArray;
   using marker_utils::createPathMarkerArray;
   using marker_utils::createPoseMarkerArray;
+  using marker_utils::createPredictedPathMarkerArray;
 
   const auto add = [this](const MarkerArray & added) {
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
   };
 
   // TODO(Sugahara): define param for target_velocity, acc_till_target_velocity, resolution,
   // stopping_time
-  size_t nearest_segment_index = motion_utils::findNearestIndex(
-    status_.pull_out_path.partial_paths.back().points,
-    planner_data_->self_odometry->pose.pose.position);
+
   const double current_velocity = planner_data_->self_odometry->twist.twist.linear.x;
   const double target_velocity = 2.0;
   const double acc_till_target_velocity = 0.5;
@@ -846,7 +844,7 @@ void StartPlannerModule::setDebugData() const
 
   const auto & ego_predicted_path = utils::createPredictedPathFromTargetVelocity(
     status_.pull_out_path.partial_paths.back().points, current_velocity, target_velocity,
-    acc_till_target_velocity, current_pose, nearest_segment_index, resolution, stopping_time);
+    acc_till_target_velocity, current_pose, resolution, stopping_time);
 
   debug_marker_.markers.clear();
   add(createPredictedPathMarkerArray(

planning/behavior_path_planner/src/utils/path_utils.cpp

@@ -129,31 +129,30 @@ PathWithLaneId resamplePathWithSpline(
 PredictedPath createPredictedPathFromTargetVelocity(
   const std::vector<PathPointWithLaneId> & following_trajectory_points,
   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)
+  const double acc_till_target_velocity, const Pose & pose, const double resolution,
+  const double stopping_time)
 {
-  [[maybe_unused]] size_t nearest_segment_index =
-    motion_utils::findNearestIndex(following_trajectory_points, pose.position);
-  std::cerr << "nearest_segment_index and nearest_seg_idx is same? : "
-            << (nearest_segment_index == nearest_seg_idx) << std::endl;
-  PredictedPath predicted_path{};
-  predicted_path.time_step = rclcpp::Duration::from_seconds(resolution);
-  predicted_path.path.reserve(
-    std::min(following_trajectory_points.size(), static_cast<size_t>(100)));
-
+  // Check if following_trajectory_points is empty
   if (following_trajectory_points.empty()) {
-    return predicted_path;
+    return PredictedPath();
   }
 
+  size_t nearest_segment_index =
+    motion_utils::findNearestIndex(following_trajectory_points, pose.position);
   FrenetPoint vehicle_pose_frenet =
-    convertToFrenetPoint(following_trajectory_points, pose.position, nearest_seg_idx);
+    convertToFrenetPoint(following_trajectory_points, pose.position, nearest_segment_index);
+
+  PredictedPath predicted_path;
+  predicted_path.time_step = rclcpp::Duration::from_seconds(resolution);
+  predicted_path.path.reserve(
+    std::min(following_trajectory_points.size(), static_cast<size_t>(100)));
 
   // add a segment to the path
   auto addSegment = [&](
                       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) {
+                      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;
@@ -164,24 +163,21 @@ PredictedPath createPredictedPathFromTargetVelocity(
   };
 
   // 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;
-  }
+  double acc_time = std::max(0.0, (target_velocity - current_velocity) / acc_till_target_velocity);
 
-  // Stopping segment, only if stopping_time is greater than zero and current_velocity is almost
-  // zero
+  // If stopping time is greater than zero and current velocity is almost zero, add stopping segment
   if (current_velocity < 0.01 && stopping_time > 0.0) {
-    addSegment(following_trajectory_points, predicted_path, 0.0, 0.0, 0.0, stopping_time, 0.0);
+    double offset = 0.0;  // Initialize offset here as it's not used before this point
+    addSegment(following_trajectory_points, predicted_path, 0.0, 0.0, 0.0, stopping_time, offset);
   }
 
-  // Acceleration segment
+  // Add acceleration segment
   double offset = 0.0;
   addSegment(
     following_trajectory_points, predicted_path, current_velocity, acc_till_target_velocity,
     stopping_time, stopping_time + acc_time, offset);
 
-  // Constant velocity segment
+  // Add 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(