fix(autoware_behavior_path_goal_planner_module): update max steer angle handling for parallel parking

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

Author: kyoichi-sugahara

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md

@@ -267,10 +267,10 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474
 
 #### Parameters geometric parallel parking
 
-| Name                    | Unit  | Type   | Description                                                                                                                         | Default value |
-| :---------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| arc_path_interval       | [m]   | double | interval between arc path points                                                                                                    | 1.0           |
-| pull_over_max_steer_rad | [rad] | double | maximum steer angle for path generation. it may not be possible to control steer up to max_steer_angle in vehicle_info when stopped | 0.35          |
+| Name                         | Unit  | Type   | Description                                                                                              | Default value |
+| :--------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ |
+| arc_path_interval            | [m]   | double | interval between arc path points                                                                         | 1.0           |
+| max_steer_angle_margin_scale | [rad] | double | scaling factor applied to the maximum steering angle (max_steer_angle) defined in vehicle_info parameter | 0.72          |
 
 #### arc forward parking
 

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

@@ -73,21 +73,19 @@
         # parallel parking path
         parallel_parking:
           path_interval: 1.0
-          max_steer_angle: 0.4 #22.9deg
+          max_steer_angle_margin_scale: 0.72
           forward:
             enable_arc_forward_parking: true
             after_forward_parking_straight_distance: 2.0
             forward_parking_velocity: 1.38
             forward_parking_lane_departure_margin: 0.0
             forward_parking_path_interval: 1.0
-            forward_parking_max_steer_angle: 0.4  # 22.9deg
           backward:
             enable_arc_backward_parking: true
             after_backward_parking_straight_distance: 2.0
             backward_parking_velocity: -1.38
             backward_parking_lane_departure_margin: 0.0
             backward_parking_path_interval: 1.0
-            backward_parking_max_steer_angle: 0.4  # 22.9deg
 
         # freespace parking
         freespace_parking:

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp

@@ -166,8 +166,11 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
 
   // 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 geometric parallel parking
+    p.parallel_parking_parameters.max_steer_angle_margin_scale =
+      node->declare_parameter<double>(ns + "max_steer_angle_margin_scale");
   }
 
   // forward parallel parking forward
@@ -182,8 +185,6 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
       node->declare_parameter<double>(ns + "forward_parking_lane_departure_margin");
     p.parallel_parking_parameters.forward_parking_path_interval =
       node->declare_parameter<double>(ns + "forward_parking_path_interval");
-    p.parallel_parking_parameters.forward_parking_max_steer_angle =
-      node->declare_parameter<double>(ns + "forward_parking_max_steer_angle");  // 20deg
   }
 
   // forward parallel parking backward
@@ -199,8 +200,6 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
       node->declare_parameter<double>(ns + "backward_parking_lane_departure_margin");
     p.parallel_parking_parameters.backward_parking_path_interval =
       node->declare_parameter<double>(ns + "backward_parking_path_interval");
-    p.parallel_parking_parameters.backward_parking_max_steer_angle =
-      node->declare_parameter<double>(ns + "backward_parking_max_steer_angle");  // 20deg
   }
 
   // freespace parking general params
@@ -557,8 +556,11 @@ void GoalPlannerModuleManager::updateModuleParams(
 
   // 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 geometric parallel parking
+    update_param<bool>(
+      parameters, ns + "max_steer_angle_margin_scale", p->max_steer_angle_margin_scale);
   }
 
   // forward parallel parking forward

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp

@@ -62,13 +62,12 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   }
 
   const auto & p = parallel_parking_parameters_;
-  const double max_steer_angle =
-    is_forward_ ? p.forward_parking_max_steer_angle : p.backward_parking_max_steer_angle;
+  const double max_steer_angle = vehicle_info_.max_steer_angle_rad * p.max_steer_angle_margin_scale;
   planner_.setTurningRadius(planner_data->parameters, max_steer_angle);
   planner_.setPlannerData(planner_data);
 
-  const bool found_valid_path =
-    planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_);
+  const bool found_valid_path = planner_.planPullOver(
+    goal_pose, road_lanes, pull_over_lanes, max_steer_angle, is_forward_, left_side_parking_);
   if (!found_valid_path) {
     return {};
   }

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

@@ -42,20 +42,19 @@ struct ParallelParkingParameters
 {
   // common
   double center_line_path_interval{0.0};
+  double max_steer_angle_margin_scale{0.0};
 
   // forward parking
   double after_forward_parking_straight_distance{0.0};
   double 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};
 
   // backward parking
   double after_backward_parking_straight_distance{0.0};
   double 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};
 
   // pull_out
   double pull_out_velocity{0.0};
@@ -70,8 +69,8 @@ class GeometricParallelParking
   bool isParking() const;
   bool planPullOver(
     const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward,
-    const bool left_side_parking);
+    const lanelet::ConstLanelets & pull_over_lanes, const double max_steer_angle,
+    const bool is_forward, const bool left_side_parking);
   bool planPullOut(
     const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
     const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start,

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

@@ -159,8 +159,8 @@ void GeometricParallelParking::clearPaths()
 
 bool GeometricParallelParking::planPullOver(
   const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward,
-  const bool left_side_parking)
+  const lanelet::ConstLanelets & pull_over_lanes, const double max_steer_angle,
+  const bool is_forward, const bool left_side_parking)
 {
   const auto & common_params = planner_data_->parameters;
   const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance
@@ -175,12 +175,10 @@ bool GeometricParallelParking::planPullOver(
   if (is_forward) {
     // When turning forward to the right, the front left goes out,
     // so reduce the steer angle at that time for seach no lane departure path.
-    // TODO(Sugahara): define in the config
     constexpr double start_pose_offset = 0.0;
     constexpr double min_steer_rad = 0.05;
     constexpr double steer_interval = 0.1;
-    for (double steer = parameters_.forward_parking_max_steer_angle; steer > min_steer_rad;
-         steer -= steer_interval) {
+    for (double steer = max_steer_angle; steer > min_steer_rad; steer -= steer_interval) {
       const double R_E_far = common_params.wheel_base / std::tan(steer);
       const auto start_pose = calcStartPose(
         arc_end_pose, road_lanes, start_pose_offset, R_E_far, is_forward, left_side_parking);

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp

@@ -66,8 +66,7 @@ std::optional<PullOutPath> GeometricPullOut::plan(
   // check if the ego is at left or right side of road lane center
   const bool left_side_start = 0 < getArcCoordinates(road_lanes, start_pose).distance;
   const double max_steer_angle =
-    vehicle_info_.max_steer_angle *
-    parallel_parking_parameters_.geometric_pull_out_max_steer_angle_margin_scale;
+    vehicle_info_.max_steer_angle_rad * parallel_parking_parameters_.max_steer_angle_margin_scale;
 
   planner_.setTurningRadius(planner_data->parameters, max_steer_angle);
   planner_.setPlannerData(planner_data);