feat(start_planner): start with acceleration

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

planning/behavior_path_planner/config/start_planner/start_planner.param.yaml

@@ -9,12 +9,12 @@
       collision_check_distance_from_end: 1.0
       # shift pull out
       enable_shift_pull_out: true
-      shift_pull_out_velocity: 2.0
-      pull_out_sampling_num: 4
       minimum_shift_pull_out_distance: 0.0
-      maximum_lateral_jerk: 2.0
-      minimum_lateral_jerk: 0.1
       deceleration_interval: 15.0
+      lateral_jerk: 0.5
+      lateral_acceleration_sampling_num: 1
+      minimum_lateral_acc: 0.15
+      maximum_lateral_acc: 0.5
       # geometric pull out
       enable_geometric_pull_out: true
       divide_pull_out_path: false

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp

@@ -33,11 +33,11 @@ struct StartPlannerParameters
   double collision_check_distance_from_end;
   // shift pull out
   bool enable_shift_pull_out;
-  double shift_pull_out_velocity;
-  int pull_out_sampling_num;
   double minimum_shift_pull_out_distance;
-  double maximum_lateral_jerk;
-  double minimum_lateral_jerk;
+  int lateral_acceleration_sampling_num;
+  double lateral_jerk;
+  double maximum_lateral_acc;
+  double minimum_lateral_acc;
   double deceleration_interval;
   // geometric pull out
   bool enable_geometric_pull_out;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1079,12 +1079,13 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam()
     declare_parameter<double>(ns + "collision_check_distance_from_end");
   // shift pull out
   p.enable_shift_pull_out = declare_parameter<bool>(ns + "enable_shift_pull_out");
-  p.shift_pull_out_velocity = declare_parameter<double>(ns + "shift_pull_out_velocity");
-  p.pull_out_sampling_num = declare_parameter<int>(ns + "pull_out_sampling_num");
   p.minimum_shift_pull_out_distance =
     declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
-  p.maximum_lateral_jerk = declare_parameter<double>(ns + "maximum_lateral_jerk");
-  p.minimum_lateral_jerk = declare_parameter<double>(ns + "minimum_lateral_jerk");
+  p.lateral_acceleration_sampling_num =
+    declare_parameter<int>(ns + "lateral_acceleration_sampling_num");
+  p.lateral_jerk = declare_parameter<double>(ns + "lateral_jerk");
+  p.maximum_lateral_acc = declare_parameter<double>(ns + "maximum_lateral_acc");
+  p.minimum_lateral_acc = declare_parameter<double>(ns + "minimum_lateral_acc");
   p.deceleration_interval = declare_parameter<double>(ns + "deceleration_interval");
   // geometric pull out
   p.enable_geometric_pull_out = declare_parameter<bool>(ns + "enable_geometric_pull_out");
@@ -1108,10 +1109,10 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam()
   p.ignore_distance_from_lane_end = declare_parameter<double>(ns + "ignore_distance_from_lane_end");
 
   // validation of parameters
-  if (p.pull_out_sampling_num < 1) {
+  if (p.lateral_acceleration_sampling_num < 1) {
     RCLCPP_FATAL_STREAM(
-      get_logger(), "pull_out_sampling_num must be positive integer. Given parameter: "
-                      << p.pull_out_sampling_num << std::endl
+      get_logger(), "lateral_acceleration_sampling_num must be positive integer. Given parameter: "
+                      << p.lateral_acceleration_sampling_num << std::endl
                       << "Terminating the program...");
     exit(EXIT_FAILURE);
   }

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

@@ -136,13 +136,15 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
 
   // rename parameter
   const double backward_path_length = common_parameter.backward_path_length;
-  const double shift_pull_out_velocity = parameter.shift_pull_out_velocity;
   const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance;
-  const double minimum_lateral_jerk = parameter.minimum_lateral_jerk;
-  const double maximum_lateral_jerk = parameter.maximum_lateral_jerk;
-  const int pull_out_sampling_num = parameter.pull_out_sampling_num;
-  const double jerk_resolution =
-    std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_out_sampling_num;
+  const double lateral_jerk = parameter.lateral_jerk;
+  const double minimum_lateral_acc = parameter.minimum_lateral_acc;
+  const double maximum_lateral_acc = parameter.maximum_lateral_acc;
+  const int lateral_acceleration_sampling_num = parameter.lateral_acceleration_sampling_num;
+  // set minimum acc for breaking loop when sampling num is 1
+  const double acc_resolution = std::max(
+    std::abs(maximum_lateral_acc - minimum_lateral_acc) / lateral_acceleration_sampling_num,
+    std::numeric_limits<double>::epsilon());
 
   // generate road lane reference path
   const auto arc_position_start = getArcCoordinates(road_lanes, start_pose);
@@ -155,43 +157,56 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   // if goal is behind start pose,
   const bool goal_is_behind = arc_position_goal.length < s_start;
   const double s_end = goal_is_behind ? road_lanes_length : arc_position_goal.length;
-  PathWithLaneId road_lane_reference_path =
-    utils::resamplePathWithSpline(route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0);
+  constexpr double RESAMPLE_INTERVAL = 1.0;
+  PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(
+    route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL);
+
+  // non_shifted_path for when shift length or pull out distance is too short
+  const PullOutPath non_shifted_path = std::invoke([&]() {
+    PullOutPath non_shifted_path{};
+    non_shifted_path.partial_paths.push_back(road_lane_reference_path);
+    non_shifted_path.start_pose = start_pose;
+    non_shifted_path.end_pose = start_pose;
+    return non_shifted_path;
+  });
 
   bool has_non_shifted_path = false;
-  for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk;
-       lateral_jerk += jerk_resolution) {
+  for (double lateral_acc = minimum_lateral_acc; lateral_acc <= maximum_lateral_acc;
+       lateral_acc += acc_resolution) {
     PathShifter path_shifter{};
+
     path_shifter.setPath(road_lane_reference_path);
 
-    // calculate after/before shifted pull out distance
-    // lateral distance from road center to start pose
-    constexpr double minimum_shift_length = 0.01;
-    const double shift_length = getArcCoordinates(road_lanes, start_pose).distance;
     // if shift length is too short, add non sifted path
-    if (std::abs(shift_length) < minimum_shift_length && !has_non_shifted_path) {
-      PullOutPath non_shifted_path{};
-      non_shifted_path.partial_paths.push_back(road_lane_reference_path);
-      non_shifted_path.start_pose = start_pose;
-      non_shifted_path.end_pose = start_pose;
+    constexpr double MINIMUM_SHIFT_LENGTH = 0.01;
+    const double shift_length = getArcCoordinates(road_lanes, start_pose).distance;
+    if (std::abs(shift_length) < MINIMUM_SHIFT_LENGTH && !has_non_shifted_path) {
       candidate_paths.push_back(non_shifted_path);
       has_non_shifted_path = true;
       continue;
     }
 
-    const double pull_out_distance = std::max(
-      PathShifter::calcLongitudinalDistFromJerk(
-        abs(shift_length), lateral_jerk, shift_pull_out_velocity),
-      minimum_shift_pull_out_distance);
+    // calculate pull out distance, longitudinal acc, terminal velocity
     const size_t shift_start_idx =
       findNearestIndex(road_lane_reference_path.points, start_pose.position);
+    const double road_velocity =
+      road_lane_reference_path.points.at(shift_start_idx).point.longitudinal_velocity_mps;
+    const double shift_time =
+      PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc);
+    const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, 1.0);
+    const double pull_out_distance = (longitudinal_acc * std::pow(shift_time, 2)) / 2.0;
+    const double terminal_velocity = longitudinal_acc * shift_time;
+
+    // clip from ego pose
     PathWithLaneId road_lane_reference_path_from_ego = road_lane_reference_path;
     road_lane_reference_path_from_ego.points.erase(
       road_lane_reference_path_from_ego.points.begin(),
       road_lane_reference_path_from_ego.points.begin() + shift_start_idx);
     // before means distance on road lane
-    const double before_shifted_pull_out_distance = calcBeforeShiftedArcLength(
-      road_lane_reference_path_from_ego, pull_out_distance, shift_length);
+    const double before_shifted_pull_out_distance = std::max(
+      minimum_shift_pull_out_distance,
+      calcBeforeShiftedArcLength(
+        road_lane_reference_path_from_ego, pull_out_distance, shift_length));
 
     // check has enough distance
     const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back());
@@ -201,6 +216,13 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
       continue;
     }
 
+    // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted
+    if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) {
+      candidate_paths.push_back(non_shifted_path);
+      has_non_shifted_path = true;
+      continue;
+    }
+
     // get shift end pose
     const auto shift_end_pose_ptr = std::invoke([&]() {
       const auto arc_position_shift_start =
@@ -223,6 +245,9 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
     shift_line.end = *shift_end_pose_ptr;
     shift_line.end_shift_length = shift_length;
     path_shifter.addShiftLine(shift_line);
+    path_shifter.setVelocity(0.0);  // initial velocity is 0
+    path_shifter.setLongitudinalAcceleration(longitudinal_acc);
+    path_shifter.setLateralAccelerationLimit(lateral_acc);
 
     // offset front side
     ShiftedPath shifted_path;
@@ -237,8 +262,8 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
     for (size_t i = 0; i < shifted_path.path.points.size(); ++i) {
       auto & point = shifted_path.path.points.at(i);
       if (i < pull_out_end_idx) {
-        point.point.longitudinal_velocity_mps = std::min(
-          point.point.longitudinal_velocity_mps, static_cast<float>(shift_pull_out_velocity));
+        point.point.longitudinal_velocity_mps =
+          std::min(point.point.longitudinal_velocity_mps, static_cast<float>(terminal_velocity));
       }
     }
     // if the end point is the goal, set the velocity to 0