From 3c2bf6ac2f783bf4415f479f0595f40ac2354a77 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 25 Oct 2023 16:00:52 +0900 Subject: [PATCH] feat(avoidance): add use_conservative_buffer_longitudinal Signed-off-by: Takayuki Murooka --- .../config/avoidance/avoidance.param.yaml | 8 +++++ .../utils/avoidance/avoidance_module_data.hpp | 2 ++ .../avoidance/avoidance_module.cpp | 34 ++++++++++++++----- .../src/scene_module/avoidance/manager.cpp | 5 +++ 4 files changed, 40 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 589233cf5a441..7c8fa1ac18348 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -37,6 +37,7 @@ avoid_margin_lateral: 1.0 # [m] safety_buffer_lateral: 0.7 # [m] safety_buffer_longitudinal: 0.0 # [m] + use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: is_target: true execute_num: 1 @@ -47,6 +48,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bus: is_target: true execute_num: 1 @@ -57,6 +59,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true trailer: is_target: true execute_num: 1 @@ -67,6 +70,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true unknown: is_target: false execute_num: 1 @@ -77,6 +81,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bicycle: is_target: false execute_num: 1 @@ -87,6 +92,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true motorcycle: is_target: false execute_num: 1 @@ -97,6 +103,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true pedestrian: is_target: false execute_num: 1 @@ -107,6 +114,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] upper_distance_for_polygon_expansion: 100.0 # [m] 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 3264ec1e9ddfd..d14a99eb80e73 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 @@ -69,6 +69,8 @@ struct ObjectParameter double safety_buffer_lateral{1.0}; double safety_buffer_longitudinal{0.0}; + + bool use_conservative_buffer_longitudinal{true}; }; struct AvoidanceParameters 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 060ad4d462744..eef9edbbf3159 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 @@ -542,7 +542,10 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat const auto o_front = data.stop_target_object.get(); const auto object_type = utils::getHighestProbLabel(o_front.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & base_link2front = planner_data_->parameters.base_link2front; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + @@ -551,7 +554,8 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat const auto variable = helper_.getSharpAvoidanceDistance( helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); const auto constant = helper_.getNominalPrepareDistance() + - object_parameter.safety_buffer_longitudinal + base_link2front; + object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal; const auto total_avoid_distance = variable + constant; dead_pose_ = calcLongitudinalOffsetPose( @@ -796,7 +800,6 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( { // To be consistent with changes in the ego position, the current shift length is considered. const auto current_ego_shift = helper_.getEgoShift(); - const auto & base_link2front = planner_data_->parameters.base_link2front; const auto & base_link2rear = planner_data_->parameters.base_link2rear; // Calculate feasible shift length @@ -834,8 +837,12 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( // calculate remaining distance. const auto prepare_distance = helper_.getNominalPrepareDistance(); - const auto constant = - object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; + const auto constant = object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal + prepare_distance; const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; const auto remaining_distance = object.longitudinal - constant; @@ -955,7 +962,12 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( AvoidLine al_avoid; { - const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; + const auto offset = + object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; const auto path_front_to_ego = avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); @@ -2727,7 +2739,6 @@ void AvoidanceModule::updateAvoidanceDebugData( double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { const auto & p = parameters_; - const auto & base_link2front = planner_data_->parameters.base_link2front; const auto & vehicle_width = planner_data_->parameters.vehicle_width; // D5 @@ -2743,7 +2754,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const // D2: min_avoid_distance // D3: longitudinal_avoid_margin_front (margin + D5) // D4: o_front.longitudinal - // D5: base_link2front + // D5: additional_buffer_longitudinal (base_link2front or 0 depending on the + // use_conservative_buffer_longitudinal) const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); @@ -2752,8 +2764,12 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto variable = helper_.getMinAvoidanceDistance( helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + - base_link2front + p->stop_buffer; + additional_buffer_longitudinal + p->stop_buffer; return object.longitudinal - std::min(variable + constant, p->stop_max_distance); } 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 8900e2b3e827c..871932e1e2232 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -79,6 +79,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); param.safety_buffer_longitudinal = getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); + param.use_conservative_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); return param; }; @@ -312,6 +314,9 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); + updateParam( + parameters, ns + "use_conservative_buffer_longitudinal", + config.use_conservative_buffer_longitudinal); }; {