Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(dynamic_avoidance): add margins for avoidance #3759

Merged
merged 4 commits into from
May 19, 2023
Merged

feat(dynamic_avoidance): add margins for avoidance #3759

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
302ded2
feat(dynamic_avoidance): add margins for avoidance
takayuki5168 May 18, 2023
b3b6f4f
update
takayuki5168 May 18, 2023
00b036d
reconfigurable parameters
takayuki5168 May 18, 2023
a68fc23
fix
takayuki5168 May 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 15 additions & 4 deletions planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,18 @@
min_obstacle_vel: 1.0 # [m/s]

drivable_area_generation:
lat_offset_from_obstacle: 2.0 # [m]
time_to_avoid_same_directional_object: 5.0 # [s]
time_to_avoid_opposite_directional_object: 6.0 # [s]
max_lat_offset_to_avoid: 2.0 # [m]
lat_offset_from_obstacle: 1.3 # [m]
max_lat_offset_to_avoid: 0.5 # [m]

# for same directional object
overtaking_object:
max_time_to_collision: 3.0 # [s]
start_duration_to_avoid: 4.0 # [s]
end_duration_to_avoid: 5.0 # [s]
duration_to_hold_avoidance: 3.0 # [s]

# for opposite directional object
oncoming_object:
max_time_to_collision: 3.0 # [s]
start_duration_to_avoid: 9.0 # [s]
end_duration_to_avoid: 0.0 # [s]
25 changes: 15 additions & 10 deletions ...ng/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,18 @@ Then the motion planner, in detail obstacle_avoidance_planner, will generate an

### Parameters

| Name | Unit | Type | Description | Default value |
| :----------------------------------------------------------------- | :---- | :----- | :---------------------------------------------------------- | :------------ |
| target_object.car | [-] | bool | The flag whether to avoid cars or not | true |
| target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true |
| ... | [-] | bool | ... | ... |
| target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 |
| drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 |
| drivable_area_generation.time_to_avoid_same_directional_object | [s] | double | Elapsed time for avoiding the same directional obstacle | 5.0 |
| drivable_area_generation.time_to_avoid_opposite_directional_object | [s] | double | Elapsed time for avoiding the opposite directional obstacle | 6.0 |
| drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 |
| Name | Unit | Type | Description | Default value |
| :-------------------------------------------------------------------- | :---- | :----- | :--------------------------------------------------------- | :------------ |
| target_object.car | [-] | bool | The flag whether to avoid cars or not | true |
| target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true |
| ... | [-] | bool | ... | ... |
| target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 |
| drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 |
| drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 |
| drivable_area_generation.overtaking_object.max_time_to_collision | [s] | double | Maximum value when calculating time to collision | 3.0 |
| drivable_area_generation.overtaking_object.start_duration_to_avoid | [s] | double | Duration to consider avoidance before passing by obstacles | 4.0 |
| drivable_area_generation.overtaking_object.end_duration_to_avoid | [s] | double | Duration to consider avoidance after passing by obstacles | 5.0 |
| drivable_area_generation.overtaking_object.duration_to_hold_avoidance | [s] | double | Duration to hold avoidance after passing by obstacles | 3.0 |
| drivable_area_generation.oncoming_object.max_time_to_collision | [s] | double | Maximum value when calculating time to collision | 3.0 |
| drivable_area_generation.oncoming_object.start_duration_to_avoid | [s] | double | Duration to consider avoidance before passing by obstacles | 9.0 |
| drivable_area_generation.oncoming_object.end_duration_to_avoid | [s] | double | Duration to consider avoidance after passing by obstacles | 0.0 |
11 changes: 9 additions & 2 deletions ...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,16 @@ struct DynamicAvoidanceParameters

// drivable area generation
double lat_offset_from_obstacle{0.0};
double time_to_avoid_same_directional_object{0.0};
double time_to_avoid_opposite_directional_object{0.0};
double max_lat_offset_to_avoid{0.0};

double max_time_to_collision_overtaking_object{0.0};
double start_duration_to_avoid_overtaking_object{0.0};
double end_duration_to_avoid_overtaking_object{0.0};
double duration_to_hold_avoidance_overtaking_object{0.0};

double max_time_to_collision_oncoming_object{0.0};
double start_duration_to_avoid_oncoming_object{0.0};
double end_duration_to_avoid_oncoming_object{0.0};
};

class DynamicAvoidanceModule : public SceneModuleInterface
Expand Down
20 changes: 16 additions & 4 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -708,11 +708,23 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
{ // drivable_area_generation
std::string ns = "dynamic_avoidance.drivable_area_generation.";
p.lat_offset_from_obstacle = declare_parameter<double>(ns + "lat_offset_from_obstacle");
p.time_to_avoid_same_directional_object =
declare_parameter<double>(ns + "time_to_avoid_same_directional_object");
p.time_to_avoid_opposite_directional_object =
declare_parameter<double>(ns + "time_to_avoid_opposite_directional_object");
p.max_lat_offset_to_avoid = declare_parameter<double>(ns + "max_lat_offset_to_avoid");

p.max_time_to_collision_overtaking_object =
declare_parameter<double>(ns + "overtaking_object.max_time_to_collision");
p.start_duration_to_avoid_overtaking_object =
declare_parameter<double>(ns + "overtaking_object.start_duration_to_avoid");
p.end_duration_to_avoid_overtaking_object =
declare_parameter<double>(ns + "overtaking_object.end_duration_to_avoid");
p.duration_to_hold_avoidance_overtaking_object =
declare_parameter<double>(ns + "overtaking_object.duration_to_hold_avoidance");

p.max_time_to_collision_oncoming_object =
declare_parameter<double>(ns + "oncoming_object.max_time_to_collision");
p.start_duration_to_avoid_oncoming_object =
declare_parameter<double>(ns + "oncoming_object.start_duration_to_avoid");
p.end_duration_to_avoid_oncoming_object =
declare_parameter<double>(ns + "oncoming_object.end_duration_to_avoid");
}

return p;
Expand Down
29 changes: 17 additions & 12 deletions ...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -396,16 +396,20 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
return signed_lon_length / relative_velocity;
}();

if (time_to_collision < 0) {
if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) {
return std::nullopt;
}

const double limited_time_to_collision = std::min(3.0, time_to_collision);
if (0 <= object.path_projected_vel) {
const double limited_time_to_collision =
std::min(parameters_->max_time_to_collision_overtaking_object, time_to_collision);
return std::make_pair(
raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision,
raw_max_obj_lon_offset + object.path_projected_vel * limited_time_to_collision);
}

const double limited_time_to_collision =
std::min(parameters_->max_time_to_collision_oncoming_object, time_to_collision);
return std::make_pair(
raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision,
raw_max_obj_lon_offset);
Expand All @@ -418,21 +422,22 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
const double max_obj_lon_offset = obj_lon_offset->second;

// calculate bound start and end index
const double length_to_avoid = [&]() {
if (0.0 <= object.path_projected_vel) {
return object.path_projected_vel * parameters_->time_to_avoid_same_directional_object;
}
return object.path_projected_vel * parameters_->time_to_avoid_opposite_directional_object;
}();
const bool is_object_overtaking = (0.0 <= object.path_projected_vel);
const double start_length_to_avoid =
std::abs(object.path_projected_vel) *
(is_object_overtaking ? parameters_->start_duration_to_avoid_overtaking_object
: parameters_->start_duration_to_avoid_oncoming_object);
const double end_length_to_avoid =
std::abs(object.path_projected_vel) * (is_object_overtaking
? parameters_->end_duration_to_avoid_overtaking_object
: parameters_->end_duration_to_avoid_oncoming_object);
const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint(
obj_seg_idx, min_obj_lon_offset + (length_to_avoid < 0 ? length_to_avoid : 0.0),
path_for_bound.points);
obj_seg_idx, min_obj_lon_offset - start_length_to_avoid, path_for_bound.points);
const size_t updated_obj_seg_idx =
(lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1
: obj_seg_idx;
const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint(
updated_obj_seg_idx, max_obj_lon_offset + (0 <= length_to_avoid ? length_to_avoid : 0.0),
path_for_bound.points);
updated_obj_seg_idx, max_obj_lon_offset + end_length_to_avoid, path_for_bound.points);

if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) {
// NOTE: The obstacle is longitudinally out of the ego's trajectory.
Expand Down
46 changes: 44 additions & 2 deletions planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,52 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
[[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters)
{
using tier4_autoware_utils::updateParam;

auto & p = parameters_;

[[maybe_unused]] std::string ns = name_ + ".";
{ // target object
const std::string ns = "dynamic_avoidance.target_object.";

updateParam<bool>(parameters, ns + "car", p->avoid_car);
updateParam<bool>(parameters, ns + "truck", p->avoid_truck);
updateParam<bool>(parameters, ns + "bus", p->avoid_bus);
updateParam<bool>(parameters, ns + "trailer", p->avoid_trailer);
updateParam<bool>(parameters, ns + "unknown", p->avoid_unknown);
updateParam<bool>(parameters, ns + "bicycle", p->avoid_bicycle);
updateParam<bool>(parameters, ns + "motorcycle", p->avoid_motorcycle);
updateParam<bool>(parameters, ns + "pedestrian", p->avoid_pedestrian);

updateParam<double>(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel);
}

{ // drivable_area_generation
const std::string ns = "dynamic_avoidance.drivable_area_generation.";

updateParam<double>(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle);
updateParam<double>(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid);

updateParam<double>(
parameters, ns + "overtaking_object.max_time_to_collision",
p->max_time_to_collision_overtaking_object);
updateParam<double>(
parameters, ns + "overtaking_object.start_duration_to_avoid",
p->start_duration_to_avoid_overtaking_object);
updateParam<double>(
parameters, ns + "overtaking_object.end_duration_to_avoid",
p->end_duration_to_avoid_overtaking_object);
updateParam<double>(
parameters, ns + "overtaking_object.duration_to_hold_avoidance",
p->duration_to_hold_avoidance_overtaking_object);

updateParam<double>(
parameters, ns + "oncoming_object.max_time_to_collision",
p->max_time_to_collision_oncoming_object);
updateParam<double>(
parameters, ns + "oncoming_object.start_duration_to_avoid",
p->start_duration_to_avoid_oncoming_object);
updateParam<double>(
parameters, ns + "oncoming_object.end_duration_to_avoid",
p->end_duration_to_avoid_oncoming_object);
}

std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) {
m->updateModuleParams(p);
Expand Down