From 55a467cc4dc2944dd684b6dc65b6b50bd4d4737e Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 8 Aug 2023 18:42:11 +0900 Subject: [PATCH] feat(avoidance): make it selectable avoidance policy Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 21 ++++++++++++--- .../utils/avoidance/avoidance_module_data.hpp | 16 ++++++++++-- .../avoidance/avoidance_module.cpp | 26 ++++++++++++++----- .../src/scene_module/avoidance/manager.cpp | 17 +++++++++--- .../src/utils/avoidance/utils.cpp | 2 +- 5 files changed, 66 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 22de79f4dc7d0..ac3840baa1c4c 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -175,16 +175,29 @@ max_distance: 20.0 # [m] stop_buffer: 1.0 # [m] - constraints: - # vehicle slows down under longitudinal constraints - use_constraints_for_decel: false # [-] + policy: + # 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. + # "reliable": insert stop or slow down point with ignoring decel/jerk constraints. + # make it possible to increase chance to avoid but uncomfortable deceleration maybe happen. + deceleration: "best_effort" # [-] + # policy for avoidance lateral margin. select "best_effort" or "reliable". + # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal + # margin to avoid. + # "reliable": module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid + # with expected lateral margin. + lateral_margin: "best_effort" # [-] + # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. + use_shorten_margin_immediately: true # [-] + constraints: # lateral constraints lateral: velocity: [1.0, 1.38, 11.1] # [m/s] max_accel_values: [0.5, 0.5, 0.5] # [m/ss] min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] - max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] # longitudinal constraints longitudinal: 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 9a3f73855e0f7..94b914b65a067 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 @@ -113,8 +113,11 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; - // constrains - bool use_constraints_for_decel{false}; + // // constrains + // bool use_constraints_for_decel{false}; + + // // policy + // bool use_relaxed_margin_immediately{false}; // max deceleration for double max_deceleration; @@ -274,6 +277,15 @@ struct AvoidanceParameters // For shift line generation process. Remove sharp(=jerky) shift line. double sharp_shift_filter_threshold; + // policy + bool use_shorten_margin_immediately{false}; + + // policy + std::string policy_deceleration{"best_effort"}; + + // policy + std::string policy_lateral_margin{"best_effort"}; + // target velocity matrix std::vector velocity_map; 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 e3310ccf57bdd..92f586a1ca70a 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 @@ -95,6 +95,11 @@ bool isDrivingSameLane( return !same_ids.empty(); } + +bool isBestEffort(const std::string & policy) +{ + return policy == "best_effort"; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -617,7 +622,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif return; } - insertPrepareVelocity(path); + // insertPrepareVelocity(path); switch (data.state) { case AvoidanceState::NOT_AVOID: { @@ -625,20 +630,20 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif } case AvoidanceState::YIELD: { insertYieldVelocity(path); - insertWaitPoint(parameters_->use_constraints_for_decel, path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); removeRegisteredShiftLines(); break; } case AvoidanceState::AVOID_PATH_NOT_READY: { - insertWaitPoint(parameters_->use_constraints_for_decel, path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } case AvoidanceState::AVOID_PATH_READY: { - insertWaitPoint(parameters_->use_constraints_for_decel, path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } case AvoidanceState::AVOID_EXECUTE: { - insertStopPoint(parameters_->use_constraints_for_decel, path); + insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); break; } default: @@ -836,6 +841,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto avoiding_shift = desire_shift_length - current_ego_shift; const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift); + if (!isBestEffort(parameters_->policy_lateral_margin)) { + return desire_shift_length; + } + // ego already has enough positive shift. const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3; if (is_object_on_right && has_enough_positive_shift) { @@ -848,6 +857,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( return desire_shift_length; } + // don't relax shift length since it can stop in front of the object. + if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) { + return desire_shift_length; + } + // calculate remaining distance. const auto prepare_distance = helper_.getNominalPrepareDistance(); const auto constant = @@ -884,7 +898,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } // avoidance distance is not enough. unavoidable. - if (!parameters_->use_constraints_for_decel) { + if (!isBestEffort(parameters_->policy_deceleration)) { object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; return boost::none; } 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 a0ae98276c05e..be8c610611422 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -191,10 +191,21 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.stop_buffer = get_parameter(node, ns + "stop_buffer"); } - // constraints + // policy { - std::string ns = "avoidance.constraints."; - p.use_constraints_for_decel = get_parameter(node, ns + "use_constraints_for_decel"); + std::string ns = "avoidance.policy."; + p.policy_deceleration = get_parameter(node, ns + "deceleration"); + p.policy_lateral_margin = get_parameter(node, ns + "lateral_margin"); + p.use_shorten_margin_immediately = + get_parameter(node, ns + "use_shorten_margin_immediately"); + + if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + + if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } } // constraints (longitudinal) diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 58beaf9cf3a87..ed38cc663bc84 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -725,7 +725,7 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters) { - if (!parameters->use_constraints_for_decel) { + if (parameters->policy_deceleration == "reliable") { object_data.is_stoppable = true; return; }