diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 3d0a334cedf84..b2f6aca20d533 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -186,11 +186,18 @@ # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] - remain_buffer_distance: 30.0 # [m] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] nominal_avoidance_speed: 8.33 # [m/s] + # return dead line + return_dead_line: + goal: + enable: true # [-] + buffer: 30.0 # [m] + traffic_light: + enable: true # [-] + buffer: 3.0 # [m] # For yield maneuver yield: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index e881c8a0d3b18..9bed99d529899 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -112,6 +112,14 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // consider avoidance return dead line + bool enable_dead_line_for_goal{false}; + bool enable_dead_line_for_traffic_light{false}; + + // module try to return original path to keep this distance from edge point of the path. + double dead_line_buffer_for_goal{0.0}; + double dead_line_buffer_for_traffic_light{0.0}; + // max deceleration for double max_deceleration{0.0}; @@ -217,9 +225,6 @@ struct AvoidanceParameters // nominal avoidance sped double nominal_avoidance_speed{0.0}; - // module try to return original path to keep this distance from edge point of the path. - double remain_buffer_distance{0.0}; - // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. double soft_road_shoulder_margin{1.0}; @@ -514,6 +519,10 @@ struct AvoidancePlanningData bool found_avoidance_path{false}; double to_stop_line{std::numeric_limits::max()}; + + double to_start_point{std::numeric_limits::lowest()}; + + double to_return_point{std::numeric_limits::max()}; }; /* diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 7988239e68071..d3537bdc81dab 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -219,13 +219,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.avoidance.longitudinal."; p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.remain_buffer_distance = getOrDeclareParameter(*node, ns + "remain_buffer_distance"); p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); } + // avoidance maneuver (return shift dead line) + { + std::string ns = "avoidance.avoidance.return_dead_line."; + p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); + p.enable_dead_line_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.enable"); + p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); + p.dead_line_buffer_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.buffer"); + } + // yield { std::string ns = "avoidance.yield.";