From 3baada383b9337d3714938048650b6f1f134bbe5 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 22 Jul 2024 17:30:15 +0900 Subject: [PATCH] feat(start_planner): add end_pose_curvature_threshold (#7901) * feat(start_planner): add end_pose_curvature_threshold Signed-off-by: kosuke55 * Update planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md Co-authored-by: Kyoichi Sugahara * update max curvature discription Signed-off-by: kosuke55 * update readme Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 Co-authored-by: Kyoichi Sugahara --- .../README.md | 23 ++++---- .../config/start_planner.param.yaml | 1 + .../data_structs.hpp | 1 + .../src/manager.cpp | 4 ++ .../src/shift_pull_out.cpp | 57 +++++++++++++++++-- 5 files changed, 71 insertions(+), 15 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md index dba4a0cfce5e7..5b68b021f99cb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md @@ -469,17 +469,18 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral #### parameters for shift pull out -| Name | Unit | Type | Description | Default value | -| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | -| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true | -| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | -| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | +| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true | +| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | +| maximum_curvature | [1/m] | double | maximum curvature. Calculate the required pull out distance from this maximum curvature, assuming the reference path is considered a straight line and shifted by two approximate arcs. This does not compensate for curvature in a shifted path or curve. | 0.07 | +| end_pose_curvature_threshold | [1/m] | double | The curvature threshold which is used for calculating the shift pull out distance. The shift end pose is shifted forward so that curvature on shift end pose is less than this value. This is to prevent the generated path from having a large curvature when the end pose is on a curve. If a shift end pose with a curvature below the threshold is not found, the shift pull out distance is used as the distance to the point with the lowest curvature among the points beyond a certain distance. | 0.01 | ### **geometric pull out** diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index d39a320a19e14..cb34f9962c479 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -32,6 +32,7 @@ minimum_lateral_acc: 0.15 maximum_lateral_acc: 0.5 maximum_curvature: 0.07 + end_pose_curvature_threshold: 0.01 # geometric pull out enable_geometric_pull_out: true geometric_collision_check_distance_from_end: 0.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index ebdb0869545ca..5903a691d60e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -122,6 +122,7 @@ struct StartPlannerParameters double maximum_lateral_acc{0.0}; double minimum_lateral_acc{0.0}; double maximum_curvature{0.0}; // maximum curvature considered in the path generation + double end_pose_curvature_threshold{0.0}; double maximum_longitudinal_deviation{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 873bab1043485..194d75e14228e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -82,6 +82,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); + p.end_pose_curvature_threshold = + node->declare_parameter(ns + "end_pose_curvature_threshold"); p.maximum_longitudinal_deviation = node->declare_parameter(ns + "maximum_longitudinal_deviation"); // geometric pull out @@ -417,6 +419,8 @@ void StartPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc); updateParam(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); updateParam(parameters, ns + "maximum_curvature", p->maximum_curvature); + updateParam( + parameters, ns + "end_pose_curvature_threshold", p->end_pose_curvature_threshold); updateParam( parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation); updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index def7072dc0e71..a6924cb391bf7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -229,6 +229,7 @@ std::vector ShiftPullOut::calcPullOutPaths( 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 end_pose_curvature_threshold = parameters_.end_pose_curvature_threshold; const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance; const int lateral_acceleration_sampling_num = parameters_.lateral_acceleration_sampling_num; @@ -304,10 +305,58 @@ std::vector ShiftPullOut::calcPullOutPaths( // short length, take maximum with the original distance. // TODO(kosuke55): update the conversion function and get rid of the comparison with original // distance. - const double pull_out_distance_converted = calcBeforeShiftedArcLength( - road_lane_reference_path_from_ego, pull_out_distance, shift_length); - const double before_shifted_pull_out_distance = - std::max(pull_out_distance, pull_out_distance_converted); + const double pull_out_distance_converted = std::max( + pull_out_distance, calcBeforeShiftedArcLength( + road_lane_reference_path_from_ego, pull_out_distance, shift_length)); + + // Calculate the distance until the curvature at end_pose is below a certain threshold. + // This is to prevent the path curvature from becoming unnecessarily large when end_pose is on a + // curve. + const double before_shifted_pull_out_distance = std::invoke([&]() -> double { + double arc_length = 0.0; + + // If a curvature below end_pose_curvature_threshold is not found, return the distance to the + // point with the smallest curvature after pull_out_distance_converted. pull_out_distance is a + // variable to store that distance. + double pull_out_distance = pull_out_distance_converted; + double min_curvature_after_distance_converted = std::numeric_limits::max(); + + const auto curvatures_and_segment_lengths = + autoware::motion_utils::calcCurvatureAndSegmentLength( + road_lane_reference_path_from_ego.points); + + const auto update_arc_length = [&](size_t i, const auto & segment_length) { + arc_length += (i == curvatures_and_segment_lengths.size() - 1) + ? segment_length.first + segment_length.second + : segment_length.first; + }; + + const auto update_min_curvature_and_pull_out_distance = [&](double curvature) { + min_curvature_after_distance_converted = curvature; + pull_out_distance = arc_length; + }; + + for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) { + const auto & [signed_curvature, segment_length] = curvatures_and_segment_lengths[i]; + const double curvature = std::abs(signed_curvature); + update_arc_length(i, segment_length); + if (arc_length > pull_out_distance_converted) { + // update distance with minimum curvature after pull_out_distance_converted + if (curvature < min_curvature_after_distance_converted) { + update_min_curvature_and_pull_out_distance(curvature); + } + // if curvature is smaller than end_pose_curvature_threshold, return the length + if (curvature < end_pose_curvature_threshold) { + return arc_length; + } + } + } + + // if not found point with curvature below end_pose_curvature_threshold + // pull_out_distance_converted, return the distance to the point with the smallest curvature + // after pull_out_distance_converted + return pull_out_distance; + }); // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted if (