diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 7c8fa1ac18348..9ba7fb717b4bb 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -192,6 +192,10 @@ stop_buffer: 1.0 # [m] policy: + # policy for rtc request. select "shift_line" or "maneuver". + # "shift_line": request approval for each shift line. + # "maneuver": request approval for avoidance maneuver (avoid + return). + approval: "shift_line" # policy for vehicle slow down behavior. select "best_effort" or "reliable". # "best_effort": slow down deceleration & jerk are limited by constraints. # but there is a possibility that the vehicle can't stop in front of the vehicle. 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 d14a99eb80e73..c41829c8d491b 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 @@ -280,6 +280,9 @@ struct AvoidanceParameters // policy bool use_shorten_margin_immediately{false}; + // policy + std::string policy_approval{"shift_line"}; + // policy std::string policy_deceleration{"best_effort"}; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 2143e42afd7b6..20dfb994ea114 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -85,6 +85,11 @@ bool isBestEffort(const std::string & policy) { return policy == "best_effort"; } + +bool perManeuver(const std::string & policy) +{ + return policy == "maneuver"; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -2316,9 +2321,16 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat break; } - if (!is_ignore_shift(candidate)) { - return get_subsequent_shift(i); + if (is_ignore_shift(candidate)) { + continue; } + + if (perManeuver(parameters_->policy_approval)) { + return candidates; + } + + const auto new_shift_lines = get_subsequent_shift(i); + return new_shift_lines; } DEBUG_PRINT("No new shift point exists."); 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 871932e1e2232..461341ff488e6 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -218,6 +218,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // policy { std::string ns = "avoidance.policy."; + p.policy_approval = getOrDeclareParameter(*node, ns + "approval"); p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); p.use_shorten_margin_immediately =