diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index ad06a1a9f6a10..9ac23885d79bc 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -215,6 +215,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 60cf9b1244236..064df93849e11 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 @@ -282,6 +282,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 1bd4fb3fef616..41078bc6ab8f5 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 @@ -90,6 +90,11 @@ bool isBestEffort(const std::string & policy) return policy == "best_effort"; } +bool perManeuver(const std::string & policy) +{ + return policy == "maneuver"; +} + AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) { AvoidLine ret{}; @@ -2434,11 +2439,18 @@ AvoidLineArray AvoidanceModule::findNewShiftLine( break; } - if (!is_ignore_shift(candidate)) { - const auto new_shift_lines = get_subsequent_shift(i); - debug.step4_new_shift_line = new_shift_lines; - return new_shift_lines; + if (is_ignore_shift(candidate)) { + continue; } + + if (perManeuver(parameters_->policy_approval)) { + debug.step4_new_shift_line = shift_lines; + return shift_lines; + } + + const auto new_shift_lines = get_subsequent_shift(i); + debug.step4_new_shift_line = new_shift_lines; + 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 f71024dcc5034..aae523a9dc09a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -257,6 +257,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 =