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): use hatched road markings #4566

Merged
merged 4 commits into from
Aug 8, 2023
Merged

feat(dynamic_avoidance): use hatched road markings #4566

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
cbf184a
feat(dynamic_avoidance): use hatched road markings
takayuki5168 Aug 8, 2023
1a93418
add some ros parameters
takayuki5168 Aug 8, 2023
6084851
update config
takayuki5168 Aug 8, 2023
d9feac4
remove comment
takayuki5168 Aug 8, 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
20 changes: 15 additions & 5 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 @@ -3,6 +3,7 @@
dynamic_avoidance:
common:
enable_debug_info: true
use_hatched_road_markings: true

# avoidance is performed for the object type with true
target_object:
Expand All @@ -18,6 +19,7 @@
min_obstacle_vel: 0.0 # [m/s]

successive_num_to_entry_dynamic_avoidance_condition: 5
successive_num_to_exit_dynamic_avoidance_condition: 1

min_obj_lat_offset_to_ego_path: 0.0 # [m]
max_obj_lat_offset_to_ego_path: 1.0 # [m]
Expand All @@ -26,26 +28,34 @@
min_time_to_start_cut_in: 1.0 # [s]
min_lon_offset_ego_to_object: 0.0 # [m]

cut_out_object:
max_time_from_outside_ego_path: 2.0 # [s]
min_object_lat_vel: 0.3 # [m/s]

crossing_object:
min_object_vel: 1.0
max_object_angle: 1.05
min_overtaking_object_vel: 1.0
max_overtaking_object_angle: 1.05
min_oncoming_object_vel: 0.0
max_oncoming_object_angle: 0.523

front_object:
max_object_angle: 0.785

drivable_area_generation:
lat_offset_from_obstacle: 1.0 # [m]
lat_offset_from_obstacle: 0.8 # [m]
max_lat_offset_to_avoid: 0.5 # [m]
max_time_for_object_lat_shift: 2.0 # [s]
lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]

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

# for opposite directional object
oncoming_object:
max_time_to_collision: 15.0 # [s]
max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles
start_duration_to_avoid: 12.0 # [s]
end_duration_to_avoid: 0.0 # [s]
3 changes: 3 additions & 0 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 @@ -45,6 +45,7 @@ struct DynamicAvoidanceParameters
{
// common
bool enable_debug_info{true};
bool use_hatched_road_markings{true};

// obstacle types to avoid
bool avoid_car{true};
Expand Down Expand Up @@ -75,6 +76,8 @@ struct DynamicAvoidanceParameters
// drivable area generation
double lat_offset_from_obstacle{0.0};
double max_lat_offset_to_avoid{0.0};
double max_time_for_lat_shift{0.0};
double lpf_gain_for_lat_avoid_to_offset{0.0};

double max_time_to_collision_overtaking_object{0.0};
double start_duration_to_avoid_overtaking_object{0.0};
Expand Down
23 changes: 14 additions & 9 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 @@ -270,7 +270,6 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
debug_marker_.markers.clear();

const auto prev_module_path = getPreviousModuleOutput().path;
const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes;

// create obstacles to avoid (= extract from the drivable area)
std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
Expand All @@ -285,11 +284,18 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
}
}

DrivableAreaInfo current_drivable_area_info;
current_drivable_area_info.drivable_lanes =
getPreviousModuleOutput().drivable_area_info.drivable_lanes;
current_drivable_area_info.obstacles = obstacles_for_drivable_area;
current_drivable_area_info.enable_expanding_hatched_road_markings =
parameters_->use_hatched_road_markings;

BehaviorModuleOutput output;
output.path = prev_module_path;
output.drivable_area_info = utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
output.reference_path = getPreviousModuleOutput().reference_path;
output.drivable_area_info.drivable_lanes = drivable_lanes;
output.drivable_area_info.obstacles = obstacles_for_drivable_area;
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;

return output;
Expand Down Expand Up @@ -827,10 +833,9 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid(

// calculate bound min and max lateral offset
const double min_bound_lat_offset = [&]() {
constexpr double object_time_to_shift = 2.0;
const double lat_abs_offset_to_shift =
std::max(0.0, obj_normal_vel * (is_collision_left ? -1.0 : 1.0)) *
object_time_to_shift; // TODO(murooka) use rosparam
parameters_->max_time_for_lat_shift;
const double raw_min_bound_lat_offset =
min_obj_lat_abs_offset - parameters_->lat_offset_from_obstacle - lat_abs_offset_to_shift;
const double min_bound_lat_abs_offset_limit =
Expand All @@ -850,10 +855,10 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid(
return prev_object->lat_offset_to_avoid.min_value;
}();
const double filtered_min_bound_lat_offset =
prev_min_lat_avoid_to_offset
? signal_processing::lowpassFilter(
min_bound_lat_offset, *prev_min_lat_avoid_to_offset, 0.5) // TODO(murooka) use rosparam
: min_bound_lat_offset;
prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter(
min_bound_lat_offset, *prev_min_lat_avoid_to_offset,
parameters_->lpf_gain_for_lat_avoid_to_offset)
: min_bound_lat_offset;

return MinMaxValue{filtered_min_bound_lat_offset, max_bound_lat_offset};
}
Expand Down
10 changes: 10 additions & 0 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 @@ -32,6 +32,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
{ // common
std::string ns = "dynamic_avoidance.common.";
p.enable_debug_info = node->declare_parameter<bool>(ns + "enable_debug_info");
p.use_hatched_road_markings = node->declare_parameter<bool>(ns + "use_hatched_road_markings");
}

{ // target object
Expand Down Expand Up @@ -82,6 +83,10 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
std::string ns = "dynamic_avoidance.drivable_area_generation.";
p.lat_offset_from_obstacle = node->declare_parameter<double>(ns + "lat_offset_from_obstacle");
p.max_lat_offset_to_avoid = node->declare_parameter<double>(ns + "max_lat_offset_to_avoid");
p.max_time_for_lat_shift =
node->declare_parameter<double>(ns + "max_time_for_object_lat_shift");
p.lpf_gain_for_lat_avoid_to_offset =
node->declare_parameter<double>(ns + "lpf_gain_for_lat_avoid_to_offset");

p.max_time_to_collision_overtaking_object =
node->declare_parameter<double>(ns + "overtaking_object.max_time_to_collision");
Expand Down Expand Up @@ -112,6 +117,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
{ // common
const std::string ns = "dynamic_avoidance.common.";
updateParam<bool>(parameters, ns + "enable_debug_info", p->enable_debug_info);
updateParam<bool>(parameters, ns + "use_hatched_road_markings", p->use_hatched_road_markings);
}

{ // target object
Expand Down Expand Up @@ -174,6 +180,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams(

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 + "max_time_for_object_lat_shift", p->max_time_for_lat_shift);
updateParam<double>(
parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset);

updateParam<double>(
parameters, ns + "overtaking_object.max_time_to_collision",
Expand Down