add velocity parameters for clothoid parking

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>

Author: Takumi Ito

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

@@ -79,6 +79,7 @@
             enable_clothoid_forward_parking: true
             after_forward_parking_straight_distance: 2.0
             forward_parking_velocity: 1.38
+            clothoid_forward_parking_velocity: 3.00
             forward_parking_lane_departure_margin: 0.0
             forward_parking_path_interval: 1.0
             forward_parking_max_steer_angle: 0.4  # 22.9deg
@@ -88,6 +89,7 @@
             enable_clothoid_backward_parking: false # Not supported yet
             after_backward_parking_straight_distance: 2.0
             backward_parking_velocity: -1.38
+            clothoid_backward_parking_velocity: -3.00
             backward_parking_lane_departure_margin: 0.0
             backward_parking_path_interval: 1.0
             backward_parking_max_steer_angle: 0.4  # 22.9deg

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp

@@ -180,6 +180,8 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
       node->declare_parameter<double>(ns + "after_forward_parking_straight_distance");
     p.parallel_parking_parameters.forward_parking_velocity =
       node->declare_parameter<double>(ns + "forward_parking_velocity");
+    p.parallel_parking_parameters.clothoid_forward_parking_velocity =
+      node->declare_parameter<double>(ns + "clothoid_forward_parking_velocity");
     p.parallel_parking_parameters.forward_parking_lane_departure_margin =
       node->declare_parameter<double>(ns + "forward_parking_lane_departure_margin");
     p.parallel_parking_parameters.forward_parking_path_interval =
@@ -201,6 +203,8 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
       node->declare_parameter<double>(ns + "after_backward_parking_straight_distance");
     p.parallel_parking_parameters.backward_parking_velocity =
       node->declare_parameter<double>(ns + "backward_parking_velocity");
+    p.parallel_parking_parameters.clothoid_backward_parking_velocity =
+      node->declare_parameter<double>(ns + "clothoid_backward_parking_velocity");
     p.parallel_parking_parameters.backward_parking_lane_departure_margin =
       node->declare_parameter<double>(ns + "backward_parking_lane_departure_margin");
     p.parallel_parking_parameters.backward_parking_path_interval =
@@ -581,6 +585,9 @@ void GoalPlannerModuleManager::updateModuleParams(
     update_param<double>(
       parameters, ns + "forward_parking_velocity",
       p->parallel_parking_parameters.forward_parking_velocity);
+    update_param<double>(
+      parameters, ns + "clothoid_forward_parking_velocity",
+      p->parallel_parking_parameters.clothoid_forward_parking_velocity);
     update_param<double>(
       parameters, ns + "forward_parking_lane_departure_margin",
       p->parallel_parking_parameters.forward_parking_lane_departure_margin);
@@ -608,6 +615,9 @@ void GoalPlannerModuleManager::updateModuleParams(
     update_param<double>(
       parameters, ns + "backward_parking_velocity",
       p->parallel_parking_parameters.backward_parking_velocity);
+    update_param<double>(
+      parameters, ns + "clothoid_backward_parking_velocity",
+      p->parallel_parking_parameters.clothoid_backward_parking_velocity);
     update_param<double>(
       parameters, ns + "backward_parking_lane_departure_margin",
       p->parallel_parking_parameters.backward_parking_lane_departure_margin);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp

@@ -46,6 +46,7 @@ struct ParallelParkingParameters
   // forward parking
   double after_forward_parking_straight_distance{0.0};
   double forward_parking_velocity{0.0};
+  double clothoid_forward_parking_velocity{0.0};
   double forward_parking_lane_departure_margin{0.0};
   double forward_parking_path_interval{0.0};
   double forward_parking_max_steer_angle{0.0};
@@ -54,6 +55,7 @@ struct ParallelParkingParameters
   // backward parking
   double after_backward_parking_straight_distance{0.0};
   double backward_parking_velocity{0.0};
+  double clothoid_backward_parking_velocity{0.0};
   double backward_parking_lane_departure_margin{0.0};
   double backward_parking_path_interval{0.0};
   double backward_parking_max_steer_angle{0.0};

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

@@ -124,10 +124,10 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
     const double L_min =
       is_forward
         ? std::abs(
-            parameters_.forward_parking_velocity * (parameters_.forward_parking_max_steer_angle /
+            parameters_.clothoid_forward_parking_velocity * (parameters_.forward_parking_max_steer_angle /
                                                     parameters_.forward_parking_steer_rate_lim))
         : std::abs(
-            parameters_.backward_parking_velocity * (parameters_.backward_parking_max_steer_angle /
+            parameters_.clothoid_backward_parking_velocity * (parameters_.backward_parking_max_steer_angle /
                                                      parameters_.backward_parking_steer_rate_lim));
     arc_paths = planOneTrialClothoid(
       start_pose, goal_pose, R_E_far, L_min, road_lanes, pull_over_lanes, is_forward,
@@ -218,9 +218,11 @@ bool GeometricParallelParking::planPullOver(
         continue;
       }
 
+      const auto velocity = use_clothoid ? parameters_.clothoid_forward_parking_velocity 
+                                         : parameters_.forward_parking_velocity;
       const auto paths = generatePullOverPaths(
         *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking,
-        use_clothoid, end_pose_offset, parameters_.forward_parking_velocity);
+        use_clothoid, end_pose_offset, velocity);
       if (!paths.empty()) {
         paths_ = paths;
         return true;
@@ -240,9 +242,11 @@ bool GeometricParallelParking::planPullOver(
         continue;
       }
 
+      const auto velocity = use_clothoid ? parameters_.clothoid_backward_parking_velocity 
+                                         : parameters_.backward_parking_velocity;
       const auto paths = generatePullOverPaths(
         *start_pose, goal_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward,
-        left_side_parking, use_clothoid, end_pose_offset, parameters_.backward_parking_velocity);
+        left_side_parking, use_clothoid, end_pose_offset, velocity);
       if (!paths.empty()) {
         paths_ = paths;
         return true;
@@ -875,11 +879,11 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrialClothoid(
 
   // set terminal velocity and acceleration(temporary implementation)
   if (is_forward) {
-    pairs_terminal_velocity_and_accel_.emplace_back(parameters_.forward_parking_velocity, 0.0);
-    pairs_terminal_velocity_and_accel_.emplace_back(parameters_.forward_parking_velocity, 0.0);
+    pairs_terminal_velocity_and_accel_.emplace_back(parameters_.clothoid_forward_parking_velocity, 0.0);
+    pairs_terminal_velocity_and_accel_.emplace_back(parameters_.clothoid_forward_parking_velocity, 0.0);
   } else {
-    pairs_terminal_velocity_and_accel_.emplace_back(parameters_.backward_parking_velocity, 0.0);
-    pairs_terminal_velocity_and_accel_.emplace_back(parameters_.backward_parking_velocity, 0.0);
+    pairs_terminal_velocity_and_accel_.emplace_back(parameters_.clothoid_backward_parking_velocity, 0.0);
+    pairs_terminal_velocity_and_accel_.emplace_back(parameters_.clothoid_backward_parking_velocity, 0.0);
   }
 
   // set pull_over start and end pose

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp

@@ -81,7 +81,8 @@ std::optional<PullOutPath> GeometricPullOut::plan(
   // collision check with stop objects in pull out lanes
   const auto arc_path = planner_.getArcPath();
 
-  const double velocity = parallel_parking_parameters_.forward_parking_velocity;
+  const double velocity = use_clothoid_ ? parallel_parking_parameters_.clothoid_forward_parking_velocity
+                                        : parallel_parking_parameters_.forward_parking_velocity;
 
   if (parameters_.divide_pull_out_path) {
     output.partial_paths = planner_.getPaths();