Skip to content

Commit

Permalink
feat(dynamic_avoidance): use hatched road markings (#4566)
Browse files Browse the repository at this point in the history 
* feat(dynamic_avoidance): use hatched road markings

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add some ros parameters

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* remove comment

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 authored Aug 8, 2023
1 parent 7502305 commit a195b05
Show file tree
Hide file tree
Showing 4 changed files with 42 additions and 14 deletions.
20 changes: 15 additions & 5 deletions planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
View file
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
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
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
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

0 comments on commit a195b05

Please sign in to comment.