diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml index 3e8907cdccdf6..ad5fe0b123f1d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml @@ -12,64 +12,80 @@ 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] + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] truck: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h 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 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bus: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h 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 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] trailer: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h 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 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h 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 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] motorcycle: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h 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 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] pedestrian: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h 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 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 4bdcb51f1eab5..11bad993ccd3d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -72,10 +72,12 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); return param; }; diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index fc755bc4115a9..1aab806f76cc9 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -257,9 +257,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); const auto nearest_object_parameter = avoidance_parameters_->object_parameters.at(nearest_object_type); - const auto avoid_margin = - nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + - nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + const auto lateral_hard_margin = std::max( + nearest_object_parameter.lateral_hard_margin, + nearest_object_parameter.lateral_hard_margin_for_parked_vehicle); + const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor + + nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width; avoidance_helper_->setData(planner_data_); const auto shift_length = avoidance_helper_->getShiftLength( @@ -288,8 +290,10 @@ double AvoidanceByLaneChange::calcLateralOffset() const { auto additional_lat_offset{0.0}; for (const auto & [type, p] : avoidance_parameters_->object_parameters) { + const auto lateral_hard_margin = + std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle); const auto offset = - 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + 2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin; additional_lat_offset = std::max(additional_lat_offset, offset); } return additional_lat_offset; diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 5a81d7d972a0e..cd6ff681b42a9 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -27,81 +27,97 @@ car: execute_num: 1 # [-] moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] + moving_time_threshold: 2.0 # [s] + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] + envelope_buffer_margin: 0.5 # [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: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bus: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true trailer: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: -0.2 # [m] + hard_margin_for_parked_vehicle: -0.2 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bicycle: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true motorcycle: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true pedestrian: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] 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 2e1d2d717509f..86889f8ca4aac 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 @@ -69,9 +69,11 @@ struct ObjectParameter double envelope_buffer_margin{0.0}; - double avoid_margin_lateral{1.0}; + double lateral_soft_margin{1.0}; - double safety_buffer_lateral{1.0}; + double lateral_hard_margin{1.0}; + + double lateral_hard_margin_for_parked_vehicle{1.0}; double safety_buffer_longitudinal{0.0}; 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 ce75fa03e96ad..13425b3118d82 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 @@ -72,10 +72,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); param.safety_buffer_longitudinal = getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); param.use_conservative_buffer_longitudinal = diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 68cee1aa2b523..692e4260520f3 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -60,8 +60,11 @@ 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 + "lateral_margin.soft_margin", config.lateral_soft_margin); + updateParam(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin); + updateParam( + parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle", + config.lateral_hard_margin_for_parked_vehicle); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); updateParam( diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index ed3dc8d34a698..eed1f324727fb 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -635,8 +635,8 @@ void AvoidanceModule::fillDebugData( : 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 + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto variable = helper_->getSharpAvoidanceDistance( helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); @@ -1423,8 +1423,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto variable = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto & additional_buffer_longitudinal = @@ -1653,8 +1653,8 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto shift_length = helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index cd2a08489fd29..84de6e06bdf74 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -234,9 +234,12 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3; + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; const auto infeasible = std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER < - 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + 0.5 * data_->parameters.vehicle_width + lateral_hard_margin; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index e64ab28841962..943ff8bfe5c4d 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -862,10 +862,13 @@ std::optional getAvoidMargin( const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + const auto max_avoid_margin = lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; + const auto min_avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; const auto soft_lateral_distance_limit = object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; const auto hard_lateral_distance_limit = @@ -1505,8 +1508,11 @@ void fillAvoidanceNecessity( { const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = object_data.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; const auto safety_margin = - 0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor; + 0.5 * vehicle_width + lateral_hard_margin * object_data.distance_factor; const auto check_necessity = [&](const auto hysteresis_factor) { return (isOnRight(object_data) && std::abs(object_data.overhang_points.front().first) < @@ -1689,8 +1695,11 @@ void updateRoadShoulderDistance( const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(o.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = o.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; - o.avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + o.avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; } const auto extract_obstacles = generateObstaclePolygonsForDrivableArea( clip_objects, parameters, planner_data->parameters.vehicle_width / 2.0); @@ -2074,8 +2083,10 @@ std::pair separateObjectsByPath( double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; + const auto lateral_hard_margin = + std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle); const auto offset = - 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + 2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin; max_offset = std::max(max_offset, offset); }