fix(start/goal_planner): resample path and make params

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml

@@ -5,6 +5,7 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 2.0  # It must be greater than the state_machine's.
+      center_line_path_interval: 1.0
 
       # goal search
       goal_search:

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

@@ -8,6 +8,7 @@
       collision_check_distance_from_end: 1.0
       th_moving_object_velocity: 1.0
       th_distance_to_middle_of_the_road: 0.1
+      center_line_path_interval: 1.0
       # shift pull out
       enable_shift_pull_out: true
       check_shift_path_lane_departure: false

planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md

@@ -118,11 +118,12 @@ Either one is activated when all conditions are met.
 
 ## General parameters for goal_planner
 
-| Name                | Unit  | Type   | Description                                        | Default value |
-| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
-| th_arrived_distance | [m]   | double | distance threshold for arrival of path termination | 1.0           |
-| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01          |
-| th_stopped_time     | [s]   | double | time threshold for arrival of path termination     | 2.0           |
+| Name                      | Unit  | Type   | Description                                        | Default value |
+| :------------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
+| th_arrived_distance       | [m]   | double | distance threshold for arrival of path termination | 1.0           |
+| th_stopped_velocity       | [m/s] | double | velocity threshold for arrival of path termination | 0.01          |
+| th_stopped_time           | [s]   | double | time threshold for arrival of path termination     | 2.0           |
+| center_line_path_interval | [m]   | double | reference center line path point interval          | 1.0           |
 
 ## **collision check**
 

planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md

@@ -70,6 +70,7 @@ PullOutPath --o PullOutPlannerBase
 | length_ratio_for_turn_signal_deactivation_near_intersection | [m]   | double | deactivate turn signal of this module near intersection                     | 0.5           |
 | collision_check_margin                                      | [m]   | double | Obstacle collision check margin                                             | 1.0           |
 | collision_check_distance_from_end                           | [m]   | double | collision check distance from end point. currently only for pull out        | 15.0          |
+| center_line_path_interval                                   | [m]   | double | reference center line path point interval                                   | 1.0           |
 
 ## Safety check with static obstacles
 

planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp

@@ -46,6 +46,9 @@ using geometry_msgs::msg::PoseArray;
 
 struct ParallelParkingParameters
 {
+  // common
+  double center_line_path_interval{0.0};
+
   // forward parking
   double after_forward_parking_straight_distance{0.0};
   double forward_parking_velocity{0.0};

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp

@@ -45,6 +45,7 @@ struct GoalPlannerParameters
   double th_stopped_velocity{0.0};
   double th_stopped_time{0.0};
   double th_blinker_on_lateral_offset{0.0};
+  double center_line_path_interval{0.0};
 
   // goal search
   std::string search_priority;   // "efficient_path" or "close_goal"

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp

@@ -57,7 +57,6 @@ class ShiftPullOver : public PullOverPlannerBase
   LaneDepartureChecker lane_departure_checker_{};
   std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};
 
-  static constexpr double resample_interval_{1.0};
   bool left_side_parking_{true};
 };
 }  // namespace behavior_path_planner

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

@@ -41,9 +41,7 @@ class ShiftPullOut : public PullOutPlannerBase
 
   std::vector<PullOutPath> calcPullOutPaths(
     const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
-    const Pose & start_pose, const Pose & goal_pose,
-    const BehaviorPathPlannerParameters & common_parameter,
-    const behavior_path_planner::StartPlannerParameters & parameter);
+    const Pose & start_pose, const Pose & goal_pose);
 
   double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double target_after_arc_length, const double dr);

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

@@ -45,6 +45,7 @@ struct StartPlannerParameters
   double collision_check_margin{0.0};
   double collision_check_distance_from_end{0.0};
   double th_moving_object_velocity{0.0};
+  double center_line_path_interval{0.0};
 
   // shift pull out
   bool enable_shift_pull_out{false};

planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp

@@ -38,6 +38,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
     p.th_stopped_velocity = node->declare_parameter<double>(base_ns + "th_stopped_velocity");
     p.th_arrived_distance = node->declare_parameter<double>(base_ns + "th_arrived_distance");
     p.th_stopped_time = node->declare_parameter<double>(base_ns + "th_stopped_time");
+    p.center_line_path_interval =
+      node->declare_parameter<double>(base_ns + "center_line_path_interval");
   }
 
   // goal search
@@ -121,6 +123,13 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
       node->declare_parameter<double>(ns + "after_shift_straight_distance");
   }
 
+  // parallel parking common
+  {
+    const std::string ns = base_ns + "pull_over.parallel_parking.";
+    p.parallel_parking_parameters.center_line_path_interval =
+      p.center_line_path_interval;  // for geometirc parallel parking
+  }
+
   // forward parallel parking forward
   {
     const std::string ns = base_ns + "pull_over.parallel_parking.forward.";

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

@@ -47,6 +47,7 @@ StartPlannerModuleManager::StartPlannerModuleManager(
   p.collision_check_distance_from_end =
     node->declare_parameter<double>(ns + "collision_check_distance_from_end");
   p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
+  p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
   // shift pull out
   p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
   p.check_shift_path_lane_departure =
@@ -71,6 +72,8 @@ StartPlannerModuleManager::StartPlannerModuleManager(
     node->declare_parameter<double>(ns + "lane_departure_margin");
   p.parallel_parking_parameters.pull_out_max_steer_angle =
     node->declare_parameter<double>(ns + "pull_out_max_steer_angle");  // 15deg
+  p.parallel_parking_parameters.center_line_path_interval =
+    p.center_line_path_interval;  // for geometirc parallel parking
   // search start pose backward
   p.search_priority = node->declare_parameter<std::string>(
     ns + "search_priority");  // "efficient_path" or "short_back_distance"

planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp

@@ -286,8 +286,9 @@ bool GeometricParallelParking::planPullOut(
       s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose);
     const double s_end = path_end_info.first;
     const bool path_terminal_is_goal = path_end_info.second;
-    PathWithLaneId road_center_line_path =
-      planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true);
+    const PathWithLaneId road_center_line_path = utils::resamplePathWithSpline(
+      planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true),
+      parameters_.center_line_path_interval);
 
     if (road_center_line_path.points.empty()) {
       continue;
@@ -362,8 +363,10 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(
   const Pose current_pose = planner_data_->self_odometry->pose.pose;
   const auto current_arc_position = lanelet::utils::getArcCoordinates(road_lanes, current_pose);
 
-  auto path = planner_data_->route_handler->getCenterLinePath(
-    road_lanes, current_arc_position.length, start_arc_position.length, true);
+  auto path = utils::resamplePathWithSpline(
+    planner_data_->route_handler->getCenterLinePath(
+      road_lanes, current_arc_position.length, start_arc_position.length, true),
+    parameters_.center_line_path_interval);
   path.header = planner_data_->route_handler->getRouteHeader();
   if (!path.points.empty()) {
     path.points.back().point.longitudinal_velocity_mps = 0;

planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp

@@ -112,7 +112,7 @@ boost::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
 
   // generate road lane reference path to shift end
   const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline(
-    generateReferencePath(road_lanes, shift_end_pose), resample_interval_);
+    generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval);
 
   // calculate shift length
   const Pose & shift_end_pose_road_lane =

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

@@ -96,7 +96,9 @@ boost::optional<PullOutPath> FreespacePullOut::plan(const Pose & start_pose, con
   const bool path_terminal_is_goal = path_end_info.second;
 
   const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end);
-  last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path);
+  last_path = utils::resamplePathWithSpline(
+    start_planner_utils::combineReferencePath(last_path, road_center_line_path),
+    parameters_.center_line_path_interval);
 
   const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps;
   utils::correctDividedPathVelocity(partial_paths);

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

@@ -59,8 +59,7 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P
     planner_data_, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
   // find candidate paths
-  auto pull_out_paths = calcPullOutPaths(
-    *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_);
+  auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose);
   if (pull_out_paths.empty()) {
     return boost::none;
   }
@@ -160,8 +159,7 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P
 
 std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
-  const Pose & start_pose, const Pose & goal_pose,
-  const BehaviorPathPlannerParameters & common_parameter, const StartPlannerParameters & parameter)
+  const Pose & start_pose, const Pose & goal_pose)
 {
   std::vector<PullOutPath> candidate_paths{};
 
@@ -170,12 +168,16 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   }
 
   // rename parameter
-  const double forward_path_length = common_parameter.forward_path_length;
-  const double backward_path_length = common_parameter.backward_path_length;
-  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;
+  const auto & common_parameters = planner_data_->parameters;
+  const double forward_path_length = common_parameters.forward_path_length;
+  const double backward_path_length = common_parameters.backward_path_length;
+  const double lateral_jerk = parameters_.lateral_jerk;
+  const double minimum_lateral_acc = parameters_.minimum_lateral_acc;
+  const double maximum_lateral_acc = parameters_.maximum_lateral_acc;
+  const double maximum_curvature = parameters_.maximum_curvature;
+  const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance;
+  const int lateral_acceleration_sampling_num = parameters_.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,
@@ -189,9 +191,9 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   const double s_end = path_end_info.first;
   const bool path_terminal_is_goal = path_end_info.second;
 
-  constexpr double RESAMPLE_INTERVAL = 1.0;
   PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(
-    route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL);
+    route_handler.getCenterLinePath(road_lanes, s_start, s_end),
+    parameters_.center_line_path_interval);
 
   // non_shifted_path for when shift length or pull out distance is too short
   const PullOutPath non_shifted_path = std::invoke([&]() {
@@ -230,8 +232,8 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
       PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc);
     const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0);
     const auto pull_out_distance = calcPullOutLongitudinalDistance(
-      longitudinal_acc, shift_time, shift_length, parameter.maximum_curvature,
-      parameter.minimum_shift_pull_out_distance);
+      longitudinal_acc, shift_time, shift_length, maximum_curvature,
+      minimum_shift_pull_out_distance);
     const double terminal_velocity = longitudinal_acc * shift_time;
 
     // clip from ego pose
@@ -253,7 +255,9 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
       std::max(pull_out_distance, pull_out_distance_converted);
 
     // 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) {
+    if (
+      before_shifted_pull_out_distance < parameters_.center_line_path_interval &&
+      !has_non_shifted_path) {
       candidate_paths.push_back(non_shifted_path);
       has_non_shifted_path = true;
       continue;