refactoring

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

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

@@ -59,10 +59,12 @@ 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
 PredictedPath createPredictedPathFromTargetVelocity(
-  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);
+  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);
 
 Path toPath(const PathWithLaneId & input);
 

planning/behavior_path_planner/src/marker_util/debug_utilities.cpp

@@ -209,6 +209,7 @@ MarkerArray createEgoPredictedPathMarkerArray(
 
   const auto & path = ego_predicted_path.path;
 
+  // TODO(Sugahara): create foot print marker and change the color
   Marker marker = createDefaultMarker(
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
     createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(r, g, b, 0.999));

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

@@ -824,17 +824,25 @@ void StartPlannerModule::setDebugData() const
   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;
+  const Pose current_pose = planner_data_->self_odometry->pose.pose;
+  const double resolution = 0.5;
+  const double stopping_time = 1.0;
 
   const auto & ego_predicted_path = utils::createPredictedPathFromTargetVelocity(
-    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);
+    status_.pull_out_path.partial_paths.back().points, current_velocity, target_velocity,
+    acc_till_target_velocity, current_pose, nearest_segment_index, resolution, stopping_time);
 
   debug_marker_.markers.clear();
-  add(createEgoPredictedPathMarkerArray(
-    ego_predicted_path, "ego_predicted_path_start_planner", 0.9, 0.3, 0.3));
+  add(createEgoPredictedPathMarkerArray(ego_predicted_path, "ego_predicted_path", 0.9, 0.3, 0.3));
   add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3));
   add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3));
   add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3));

planning/behavior_path_planner/src/utils/path_utils.cpp

@@ -127,21 +127,22 @@ PathWithLaneId resamplePathWithSpline(
 }
 
 PredictedPath createPredictedPathFromTargetVelocity(
-  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)
+  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)
 {
   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)));
+    std::min(following_trajectory_points.size(), static_cast<size_t>(100)));
 
-  if (following_trajectory.points.empty()) {
+  if (following_trajectory_points.empty()) {
     return predicted_path;
   }
 
   FrenetPoint vehicle_pose_frenet =
-    convertToFrenetPoint(following_trajectory.points, pose.position, nearest_seg_idx);
+    convertToFrenetPoint(following_trajectory_points, pose.position, nearest_seg_idx);
 
   // add a segment to the path
   auto addSegment = [&](
@@ -164,22 +165,23 @@ PredictedPath createPredictedPathFromTargetVelocity(
     acc_time = 0.0;
   }
 
+  // TODO(Sugahara): Stopping time is considered only when the vehicle is stopping.
   // Stopping segment, only if stopping_time is greater than zero
   if (stopping_time > 0.0) {
-    addSegment(following_trajectory.points, predicted_path, 0.0, 0.0, 0.0, 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.points, predicted_path, current_velocity, acc_till_target_velocity,
+    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.points, 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;