Skip to content

Commit 3baada3

Browse files
kosuke55kyoichi-sugahara
kosuke55
and
kyoichi-sugahara
committed
feat(start_planner): add end_pose_curvature_threshold (autowarefoundation#7901)
* feat(start_planner): add end_pose_curvature_threshold Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> * update max curvature discription Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * update readme Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
1 parent 94c336f commit 3baada3

File tree

5 files changed

+71
-15
lines changed

5 files changed

+71
-15
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md

+12-11
Original file line numberDiff line numberDiff line change
@@ -469,17 +469,18 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral
469469

470470
#### parameters for shift pull out
471471

472-
| Name | Unit | Type | Description | Default value |
473-
| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
474-
| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true |
475-
| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true |
476-
| 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 |
477-
| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 |
478-
| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 |
479-
| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 |
480-
| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 |
481-
| 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 |
482-
| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 |
472+
| Name | Unit | Type | Description | Default value |
473+
| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
474+
| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true |
475+
| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true |
476+
| 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 |
477+
| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 |
478+
| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 |
479+
| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 |
480+
| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 |
481+
| 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 |
482+
| 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 |
483+
| 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 |
483484

484485
### **geometric pull out**
485486

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

+1
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
minimum_lateral_acc: 0.15
3333
maximum_lateral_acc: 0.5
3434
maximum_curvature: 0.07
35+
end_pose_curvature_threshold: 0.01
3536
# geometric pull out
3637
enable_geometric_pull_out: true
3738
geometric_collision_check_distance_from_end: 0.0

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -122,6 +122,7 @@ struct StartPlannerParameters
122122
double maximum_lateral_acc{0.0};
123123
double minimum_lateral_acc{0.0};
124124
double maximum_curvature{0.0}; // maximum curvature considered in the path generation
125+
double end_pose_curvature_threshold{0.0};
125126
double maximum_longitudinal_deviation{0.0};
126127
// geometric pull out
127128
bool enable_geometric_pull_out{false};

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
8282
p.maximum_lateral_acc = node->declare_parameter<double>(ns + "maximum_lateral_acc");
8383
p.minimum_lateral_acc = node->declare_parameter<double>(ns + "minimum_lateral_acc");
8484
p.maximum_curvature = node->declare_parameter<double>(ns + "maximum_curvature");
85+
p.end_pose_curvature_threshold =
86+
node->declare_parameter<double>(ns + "end_pose_curvature_threshold");
8587
p.maximum_longitudinal_deviation =
8688
node->declare_parameter<double>(ns + "maximum_longitudinal_deviation");
8789
// geometric pull out
@@ -417,6 +419,8 @@ void StartPlannerModuleManager::updateModuleParams(
417419
updateParam<double>(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc);
418420
updateParam<double>(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc);
419421
updateParam<double>(parameters, ns + "maximum_curvature", p->maximum_curvature);
422+
updateParam<double>(
423+
parameters, ns + "end_pose_curvature_threshold", p->end_pose_curvature_threshold);
420424
updateParam<double>(
421425
parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation);
422426
updateParam<bool>(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out);

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp

+53-4
Original file line numberDiff line numberDiff line change
@@ -229,6 +229,7 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
229229
const double minimum_lateral_acc = parameters_.minimum_lateral_acc;
230230
const double maximum_lateral_acc = parameters_.maximum_lateral_acc;
231231
const double maximum_curvature = parameters_.maximum_curvature;
232+
const double end_pose_curvature_threshold = parameters_.end_pose_curvature_threshold;
232233
const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance;
233234
const int lateral_acceleration_sampling_num = parameters_.lateral_acceleration_sampling_num;
234235

@@ -304,10 +305,58 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
304305
// short length, take maximum with the original distance.
305306
// TODO(kosuke55): update the conversion function and get rid of the comparison with original
306307
// distance.
307-
const double pull_out_distance_converted = calcBeforeShiftedArcLength(
308-
road_lane_reference_path_from_ego, pull_out_distance, shift_length);
309-
const double before_shifted_pull_out_distance =
310-
std::max(pull_out_distance, pull_out_distance_converted);
308+
const double pull_out_distance_converted = std::max(
309+
pull_out_distance, calcBeforeShiftedArcLength(
310+
road_lane_reference_path_from_ego, pull_out_distance, shift_length));
311+
312+
// Calculate the distance until the curvature at end_pose is below a certain threshold.
313+
// This is to prevent the path curvature from becoming unnecessarily large when end_pose is on a
314+
// curve.
315+
const double before_shifted_pull_out_distance = std::invoke([&]() -> double {
316+
double arc_length = 0.0;
317+
318+
// If a curvature below end_pose_curvature_threshold is not found, return the distance to the
319+
// point with the smallest curvature after pull_out_distance_converted. pull_out_distance is a
320+
// variable to store that distance.
321+
double pull_out_distance = pull_out_distance_converted;
322+
double min_curvature_after_distance_converted = std::numeric_limits<double>::max();
323+
324+
const auto curvatures_and_segment_lengths =
325+
autoware::motion_utils::calcCurvatureAndSegmentLength(
326+
road_lane_reference_path_from_ego.points);
327+
328+
const auto update_arc_length = [&](size_t i, const auto & segment_length) {
329+
arc_length += (i == curvatures_and_segment_lengths.size() - 1)
330+
? segment_length.first + segment_length.second
331+
: segment_length.first;
332+
};
333+
334+
const auto update_min_curvature_and_pull_out_distance = [&](double curvature) {
335+
min_curvature_after_distance_converted = curvature;
336+
pull_out_distance = arc_length;
337+
};
338+
339+
for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) {
340+
const auto & [signed_curvature, segment_length] = curvatures_and_segment_lengths[i];
341+
const double curvature = std::abs(signed_curvature);
342+
update_arc_length(i, segment_length);
343+
if (arc_length > pull_out_distance_converted) {
344+
// update distance with minimum curvature after pull_out_distance_converted
345+
if (curvature < min_curvature_after_distance_converted) {
346+
update_min_curvature_and_pull_out_distance(curvature);
347+
}
348+
// if curvature is smaller than end_pose_curvature_threshold, return the length
349+
if (curvature < end_pose_curvature_threshold) {
350+
return arc_length;
351+
}
352+
}
353+
}
354+
355+
// if not found point with curvature below end_pose_curvature_threshold
356+
// pull_out_distance_converted, return the distance to the point with the smallest curvature
357+
// after pull_out_distance_converted
358+
return pull_out_distance;
359+
});
311360

312361
// if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted
313362
if (

0 commit comments

Comments
 (0)