diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 92f533bc63872..5d6044df64b2f 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -28,7 +28,6 @@ hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 # [-] FOR DEVELOPER envelope_buffer_margin: 0.5 # [m] FOR DEVELOPER - use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -39,7 +38,6 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true bus: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -50,7 +48,6 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true trailer: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -61,7 +58,6 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true unknown: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -72,7 +68,6 @@ hard_margin_for_parked_vehicle: -0.2 max_expand_ratio: 0.0 envelope_buffer_margin: 0.1 - use_conservative_buffer_longitudinal: true bicycle: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -83,7 +78,6 @@ hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true motorcycle: th_moving_speed: 1.0 th_moving_time: 1.0 @@ -94,7 +88,6 @@ hard_margin_for_parked_vehicle: 0.3 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true pedestrian: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -105,7 +98,6 @@ hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] FOR DEVELOPER upper_distance_for_polygon_expansion: 100.0 # [m] FOR DEVELOPER @@ -222,6 +214,8 @@ min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] FOR DEVELOPER nominal_avoidance_speed: 8.33 # [m/s] FOR DEVELOPER + consider_front_overhang: true # [-] + consider_rear_overhang: true # [-] # return dead line return_dead_line: goal: diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index bc66598d3ee90..e74b546f31fc4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -76,8 +76,6 @@ struct ObjectParameter double lateral_hard_margin_for_parked_vehicle{1.0}; double longitudinal_margin{0.0}; - - bool use_conservative_buffer_longitudinal{true}; }; struct AvoidanceParameters @@ -191,14 +189,6 @@ struct AvoidanceParameters double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; - // when complete avoidance motion, there is a distance margin with the object - // for longitudinal direction - double longitudinal_collision_margin_min_distance{0.0}; - - // when complete avoidance motion, there is a time margin with the object - // for longitudinal direction - double longitudinal_collision_margin_time{0.0}; - // parameters for safety check area bool enable_safety_check{false}; bool check_current_lane{false}; @@ -219,6 +209,9 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; + bool consider_front_overhang{true}; + bool consider_rear_overhang{true}; + // maximum stop distance double stop_max_distance{0.0}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 2e481e7c98e44..8d95e724045ba 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -148,16 +148,19 @@ class AvoidanceHelper { const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front - : 0.0; - return object_parameter.longitudinal_margin + additional_buffer_longitudinal; + if (!parameters_->consider_front_overhang) { + return object_parameter.longitudinal_margin; + } + return object_parameter.longitudinal_margin + data_->parameters.base_link2front; } double getRearConstantDistance(const ObjectData & object) const { const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); + if (!parameters_->consider_rear_overhang) { + return object_parameter.longitudinal_margin; + } return object_parameter.longitudinal_margin + data_->parameters.base_link2rear + object.length; } diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 5b8963c7ca22d..6dbf52c0425fb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -70,8 +70,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) param.lateral_hard_margin_for_parked_vehicle = getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); param.longitudinal_margin = getOrDeclareParameter(*node, ns + "longitudinal_margin"); - param.use_conservative_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); return param; }; @@ -270,6 +268,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + p.consider_front_overhang = getOrDeclareParameter(*node, ns + "consider_front_overhang"); + p.consider_rear_overhang = getOrDeclareParameter(*node, ns + "consider_rear_overhang"); } // avoidance maneuver (return shift dead line) diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json index cd3e29b8ec0b8..79882beb805f8 100644 --- a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -104,11 +104,6 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 - }, - "use_conservative_buffer_longitudinal": { - "type": "boolean", - "description": "When set to true, the base_link2front is added to the longitudinal buffer before avoidance.", - "default": "true" } }, "required": [ @@ -171,11 +166,6 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 - }, - "use_conservative_buffer_longitudinal": { - "type": "boolean", - "description": "When set to true, the base_link2front is added to the longitudinal buffer before avoidance.", - "default": "true" } }, "required": [ @@ -238,11 +228,6 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 - }, - "use_conservative_buffer_longitudinal": { - "type": "boolean", - "description": "When set to true, the base_link2front is added to the longitudinal buffer before avoidance.", - "default": "true" } }, "required": [ @@ -305,11 +290,6 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 - }, - "use_conservative_buffer_longitudinal": { - "type": "boolean", - "description": "When set to true, the base_link2front is added to the longitudinal buffer before avoidance.", - "default": "true" } }, "required": [ @@ -372,11 +352,6 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 - }, - "use_conservative_buffer_longitudinal": { - "type": "boolean", - "description": "When set to true, the base_link2front is added to the longitudinal buffer before avoidance.", - "default": "true" } }, "required": [ @@ -439,11 +414,6 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 - }, - "use_conservative_buffer_longitudinal": { - "type": "boolean", - "description": "When set to true, the base_link2front is added to the longitudinal buffer before avoidance.", - "default": "true" } }, "required": [ @@ -506,11 +476,6 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 - }, - "use_conservative_buffer_longitudinal": { - "type": "boolean", - "description": "When set to true, the base_link2front is added to the longitudinal buffer before avoidance.", - "default": "true" } }, "required": [ @@ -573,11 +538,6 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 - }, - "use_conservative_buffer_longitudinal": { - "type": "boolean", - "description": "When set to true, the base_link2front is added to the longitudinal buffer before avoidance.", - "default": "true" } }, "required": [ @@ -1150,6 +1110,16 @@ "type": "number", "description": "Nominal avoidance speed.", "default": 8.33 + }, + "consider_front_overhang": { + "type": "boolean", + "description": "Flag to consider vehicle front overhang in shift line generation logic.", + "default": true + }, + "consider_rear_overhang": { + "type": "boolean", + "description": "Flag to consider vehicle rear overhang in shift line generation logic.", + "default": true } }, "required": [ @@ -1158,7 +1128,9 @@ "min_prepare_distance", "min_slow_down_speed", "buf_slow_down_speed", - "nominal_avoidance_speed" + "nominal_avoidance_speed", + "consider_front_overhang", + "consider_rear_overhang" ], "additionalProperties": false }, diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 3261214d1b9b6..e3391f251e55f 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -59,9 +59,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "longitudinal_margin", config.longitudinal_margin); - updateParam( - parameters, ns + "use_conservative_buffer_longitudinal", - config.use_conservative_buffer_longitudinal); }; { @@ -170,6 +167,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "min_prepare_distance", p->min_prepare_distance); updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); + updateParam(parameters, ns + "consider_front_overhang", p->consider_front_overhang); + updateParam(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang); } {