Skip to content

Commit

Permalink
feat(avoidance): use improved path shifting logic
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Jul 10, 2023
1 parent d4bb8bd commit c07e7ca
Show file tree
Hide file tree
Showing 5 changed files with 43 additions and 33 deletions.
11 changes: 5 additions & 6 deletions planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,18 +164,17 @@

# lateral constraints
lateral:
nominal_lateral_jerk: 0.2 # [m/s3]
max_lateral_jerk: 1.0 # [m/s3]
nominal_lateral_jerk: 0.2 # [m/sss]
max_lateral_jerk: 1.0 # [m/sss]
max_lateral_acceleration: 0.5 # [m/ss]

# longitudinal constraints
longitudinal:
nominal_deceleration: -1.0 # [m/ss]
nominal_jerk: 0.5 # [m/sss]
max_deceleration: -2.0 # [m/ss]
max_jerk: 1.0
# For prevention of large acceleration while avoidance
min_avoidance_speed_for_acc_prevention: 3.0 # [m/s]
max_avoidance_acceleration: 0.5 # [m/ss]
max_jerk: 1.0 # [m/sss]
max_acceleration: 1.0 # [m/ss]

target_velocity_matrix:
col_size: 2
Expand Down
28 changes: 14 additions & 14 deletions planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -717,23 +717,23 @@ namespace: `avoidance.constraints.`

namespace: `avoidance.constraints.lateral.`

| a Name | Unit | Type | Description | Default value |
| :------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------- | :------------ |
| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 |
| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 |
| nominal_lateral_jerk | [m/s3] | double | Avoidance path is generated with this jerk when there is enough distance from ego. | 0.2 |
| max_lateral_jerk | [m/s3] | double | Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego. | 1.0 |
| a Name | Unit | Type | Description | Default value |
| :----------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------- | :------------ |
| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 |
| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 |
| nominal_lateral_jerk | [m/s3] | double | Avoidance path is generated with this jerk when there is enough distance from ego. | 0.2 |
| max_lateral_jerk | [m/s3] | double | Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego. | 1.0 |
| max_lateral_acceleration | [m/s3] | double | Avoidance path gets sharp up to this accel limit when there is not enough distance from ego. | 0.5 |

namespace: `avoidance.constraints.longitudinal.`

| Name | Unit | Type | Description | Default value |
| :------------------------------------- | :------ | :----- | :-------------------------------------------------------------------------- | :------------ |
| nominal_deceleration | [m/ss] | double | Nominal deceleration limit. | -1.0 |
| nominal_jerk | [m/sss] | double | Nominal jerk limit. | 0.5 |
| max_deceleration | [m/ss] | double | Max decelerate limit. | -2.0 |
| max_jerk | [m/sss] | double | Max jerk limit. | 1.0 |
| min_avoidance_speed_for_acc_prevention | [m] | double | Minimum speed limit to be applied to prevent acceleration during avoidance. | 3.0 |
| max_avoidance_acceleration | [m/ss] | double | Maximum acceleration during avoidance. | 0.5 |
| Name | Unit | Type | Description | Default value |
| :------------------- | :------ | :----- | :------------------------------------- | :------------ |
| nominal_deceleration | [m/ss] | double | Nominal deceleration limit. | -1.0 |
| nominal_jerk | [m/sss] | double | Nominal jerk limit. | 0.5 |
| max_deceleration | [m/ss] | double | Max decelerate limit. | -2.0 |
| max_jerk | [m/sss] | double | Max jerk limit. | 1.0 |
| max_acceleration | [m/ss] | double | Maximum acceleration during avoidance. | 1.0 |

(\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value.

Expand Down
14 changes: 6 additions & 8 deletions ...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,9 @@ struct AvoidanceParameters
// comfortable jerk
double nominal_jerk;

// To prevent large acceleration while avoidance.
double max_acceleration;

// upper distance for envelope polygon expansion.
double upper_distance_for_polygon_expansion;

Expand Down Expand Up @@ -245,19 +248,14 @@ struct AvoidanceParameters
// if the avoidance path exceeds this lateral jerk, it will be not used anymore.
double max_lateral_jerk;

// To prevent large acceleration while avoidance.
double max_lateral_acceleration;

// For the compensation of the detection lost. Once an object is observed, it is registered and
// will be used for planning from the next time. If the object is not observed, it counts up the
// lost_count and the registered object will be removed when the count exceeds this max count.
double object_last_seen_threshold;

// For velocity planning to avoid acceleration during avoidance.
// Speeds smaller than this are not inserted.
double min_avoidance_speed_for_acc_prevention;

// To prevent large acceleration while avoidance. The max velocity is limited with this
// acceleration.
double max_avoidance_acceleration;

// The avoidance path generation is performed when the shift distance of the
// avoidance points is greater than this threshold.
// In multiple targets case: if there are multiple vehicles in a row to be avoided, no new
Expand Down
17 changes: 15 additions & 2 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2717,8 +2717,9 @@ void AvoidanceModule::addNewShiftLines(
}

const auto current_shift_lines = path_shifter.getShiftLines();
const auto new_shift_length = new_shift_lines.front().end_shift_length;
const auto new_shift_end_idx = new_shift_lines.front().end_idx;
const auto front_new_shift_line = new_shift_lines.front();
const auto new_shift_length = front_new_shift_line.end_shift_length;
const auto new_shift_end_idx = front_new_shift_line.end_idx;

DEBUG_PRINT("min_start_idx = %lu", min_start_idx);

Expand Down Expand Up @@ -2756,7 +2757,19 @@ void AvoidanceModule::addNewShiftLines(
future.push_back(sl);
}

const double road_velocity =
avoidance_data_.reference_path.points.at(front_new_shift_line.start_idx)
.point.longitudinal_velocity_mps;
const double shift_time = PathShifter::calcShiftTimeFromJerk(
front_new_shift_line.getRelativeLength(), parameters_->max_lateral_jerk,
parameters_->max_lateral_acceleration);
const double longitudinal_acc =
std::clamp(road_velocity / shift_time, 0.0, parameters_->max_acceleration);

path_shifter.setShiftLines(future);
path_shifter.setVelocity(getEgoSpeed());
path_shifter.setLongitudinalAcceleration(longitudinal_acc);
path_shifter.setLateralAccelerationLimit(parameters_->max_lateral_acceleration);
}

AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidates) const
Expand Down
6 changes: 3 additions & 3 deletions planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,16 +195,15 @@ AvoidanceModuleManager::AvoidanceModuleManager(
p.nominal_jerk = get_parameter<double>(node, ns + "nominal_jerk");
p.max_deceleration = get_parameter<double>(node, ns + "max_deceleration");
p.max_jerk = get_parameter<double>(node, ns + "max_jerk");
p.min_avoidance_speed_for_acc_prevention =
get_parameter<double>(node, ns + "min_avoidance_speed_for_acc_prevention");
p.max_avoidance_acceleration = get_parameter<double>(node, ns + "max_avoidance_acceleration");
p.max_acceleration = get_parameter<double>(node, ns + "max_acceleration");
}

// constraints (lateral)
{
std::string ns = "avoidance.constraints.lateral.";
p.nominal_lateral_jerk = get_parameter<double>(node, ns + "nominal_lateral_jerk");
p.max_lateral_jerk = get_parameter<double>(node, ns + "max_lateral_jerk");
p.max_lateral_acceleration = get_parameter<double>(node, ns + "max_lateral_acceleration");
}

// velocity matrix
Expand Down Expand Up @@ -307,6 +306,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
const std::string ns = "avoidance.constrains.lateral.";
updateParam<double>(parameters, ns + "nominal_lateral_jerk", p->nominal_lateral_jerk);
updateParam<double>(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk);
updateParam<double>(parameters, ns + "max_lateral_acceleration", p->max_lateral_acceleration);
}

{
Expand Down

0 comments on commit c07e7ca

Please sign in to comment.