Skip to content

Commit

Permalink
feat(start_planner): add end_pose_curvature_threshold (autowarefounda…
Browse files Browse the repository at this point in the history 
…tion#7901)

* feat(start_planner): add end_pose_curvature_threshold

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

* Update planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md

Co-authored-by: Kyoichi Sugahara <[email protected]>

* update max curvature discription

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

* update readme

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

---------

Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: Kyoichi Sugahara <[email protected]>
  • Loading branch information
@kosuke55 @kyoichi-sugahara
kosuke55 and kyoichi-sugahara committed Jul 29, 2024
1 parent 94c336f commit 3baada3
Show file tree
Hide file tree
Showing 5 changed files with 71 additions and 15 deletions.
23 changes: 12 additions & 11 deletions ...ing/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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**

Expand Down
1 change: 1 addition & 0 deletions ..._path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions ...start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down
4 changes: 4 additions & 0 deletions planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.maximum_lateral_acc = node->declare_parameter<double>(ns + "maximum_lateral_acc");
p.minimum_lateral_acc = node->declare_parameter<double>(ns + "minimum_lateral_acc");
p.maximum_curvature = node->declare_parameter<double>(ns + "maximum_curvature");
p.end_pose_curvature_threshold =
node->declare_parameter<double>(ns + "end_pose_curvature_threshold");
p.maximum_longitudinal_deviation =
node->declare_parameter<double>(ns + "maximum_longitudinal_deviation");
// geometric pull out
Expand Down Expand Up @@ -417,6 +419,8 @@ void StartPlannerModuleManager::updateModuleParams(
updateParam<double>(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc);
updateParam<double>(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc);
updateParam<double>(parameters, ns + "maximum_curvature", p->maximum_curvature);
updateParam<double>(
parameters, ns + "end_pose_curvature_threshold", p->end_pose_curvature_threshold);
updateParam<double>(
parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation);
updateParam<bool>(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out);
Expand Down
57 changes: 53 additions & 4 deletions .../behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,6 +229,7 @@ std::vector<PullOutPath> 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;

Expand Down Expand Up @@ -304,10 +305,58 @@ std::vector<PullOutPath> 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<double>::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 (
Expand Down

0 comments on commit 3baada3

Please sign in to comment.