From c9e32598bda86c865e6bcb37287d154fad08a324 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 9 Aug 2023 03:56:13 +0900 Subject: [PATCH] feat(dynamic_avoidance): use hatched road markings (#4566) * feat(dynamic_avoidance): use hatched road markings Signed-off-by: Takayuki Murooka * add some ros parameters Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * remove comment Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance.param.yaml | 20 ++++++++++++---- .../dynamic_avoidance_module.hpp | 3 +++ .../dynamic_avoidance_module.cpp | 23 +++++++++++-------- .../dynamic_avoidance/manager.cpp | 10 ++++++++ 4 files changed, 42 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 336879185f48e..701f05eb89ba1 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -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: @@ -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] @@ -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] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 087a39e87393a..de41fe9359bc2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -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}; @@ -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}; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index e8570f0e310cf..b5ed7cc217a56 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -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 obstacles_for_drivable_area; @@ -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; @@ -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 = @@ -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}; } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index e686719392e14..e378d43f6497c 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -32,6 +32,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { // common std::string ns = "dynamic_avoidance.common."; p.enable_debug_info = node->declare_parameter(ns + "enable_debug_info"); + p.use_hatched_road_markings = node->declare_parameter(ns + "use_hatched_road_markings"); } { // target object @@ -82,6 +83,10 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( std::string ns = "dynamic_avoidance.drivable_area_generation."; p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid"); + p.max_time_for_lat_shift = + node->declare_parameter(ns + "max_time_for_object_lat_shift"); + p.lpf_gain_for_lat_avoid_to_offset = + node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); @@ -112,6 +117,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // common const std::string ns = "dynamic_avoidance.common."; updateParam(parameters, ns + "enable_debug_info", p->enable_debug_info); + updateParam(parameters, ns + "use_hatched_road_markings", p->use_hatched_road_markings); } { // target object @@ -174,6 +180,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); + updateParam( + parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift); + updateParam( + parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); updateParam( parameters, ns + "overtaking_object.max_time_to_collision",