diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index f7867ea647e13..79aca0867cce9 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -33,6 +33,7 @@ moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] + avoid_margin_lateral: 1.0 # [m] safety_buffer_lateral: 0.7 # [m] safety_buffer_longitudinal: 0.0 # [m] truck: @@ -41,6 +42,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bus: @@ -49,6 +51,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 trailer: @@ -57,6 +60,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 unknown: @@ -65,6 +69,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bicycle: @@ -73,6 +78,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 motorcycle: @@ -81,6 +87,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 pedestrian: @@ -89,6 +96,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 lower_distance_for_polygon_expansion: 30.0 # [m] @@ -124,7 +132,6 @@ avoidance: # avoidance lateral parameters lateral: - lateral_collision_margin: 1.0 # [m] lateral_execution_threshold: 0.499 # [m] lateral_small_shift_threshold: 0.101 # [m] road_shoulder_safety_margin: 0.3 # [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 3dd5668f43ddf..187dca71ae6c8 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 @@ -58,6 +58,8 @@ struct ObjectParameter double envelope_buffer_margin{0.0}; + double avoid_margin_lateral{1.0}; + double safety_buffer_lateral{1.0}; double safety_buffer_longitudinal{0.0}; @@ -159,9 +161,6 @@ struct AvoidanceParameters // force avoidance double threshold_time_force_avoidance_for_stopped_vehicle; - // we want to keep this lateral margin when avoiding - double lateral_collision_margin; - // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction double longitudinal_collision_margin_min_distance; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 229ae0eb679ea..f7336dad3f895 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -586,6 +586,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() param.moving_time_threshold = declare_parameter(ns + "moving_time_threshold"); param.max_expand_ratio = declare_parameter(ns + "max_expand_ratio"); param.envelope_buffer_margin = declare_parameter(ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = declare_parameter(ns + "avoid_margin_lateral"); param.safety_buffer_lateral = declare_parameter(ns + "safety_buffer_lateral"); param.safety_buffer_longitudinal = declare_parameter(ns + "safety_buffer_longitudinal"); @@ -649,7 +650,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; - p.lateral_collision_margin = declare_parameter(ns + "lateral_collision_margin"); p.road_shoulder_safety_margin = declare_parameter(ns + "road_shoulder_safety_margin"); p.lateral_execution_threshold = declare_parameter(ns + "lateral_execution_threshold"); p.lateral_small_shift_threshold = 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 ac98eb5a2b59b..8c0d6d4acd48a 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 @@ -620,7 +620,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + - parameters_->lateral_collision_margin + 0.5 * vehicle_width; + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto variable = helper_.getSharpAvoidanceDistance( helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); @@ -3285,7 +3285,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto object_parameter = parameters_->object_parameters.at(t); const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - p->lateral_collision_margin + 0.5 * vehicle_width; + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto variable = helper_.getMinimumAvoidanceDistance( helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto constant = 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 321a5a72cc7b7..90dbfa960dbfe 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -52,6 +52,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "moving_time_threshold", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); + updateParam(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral); updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); @@ -82,7 +83,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorlateral_execution_threshold); updateParam( parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold); - updateParam(parameters, ns + "lateral_collision_margin", p->lateral_collision_margin); updateParam( parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin); } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 06ca1c3cc979e..8d33faf82e645 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -894,7 +894,7 @@ void filterTargetObjects( // calculate avoid_margin dynamically // NOTE: This calculation must be after calculating to_road_shoulder_distance. const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor + - parameters->lateral_collision_margin + 0.5 * vehicle_width; + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const double min_safety_lateral_distance = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; const auto max_allowable_lateral_distance =