diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 9ecc1a63b55b0..26f53cc91c4cf 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -142,7 +142,8 @@ time_resolution: 0.5 # [s] time_horizon: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_hysteresis_factor: 2.0 # [-] + hysteresis_factor_expand_rate: 2.0 # [-] + hysteresis_factor_safe_count: 10 # [-] # rss parameters expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 7889e91767155..096ae61a9432e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -556,6 +556,8 @@ class AvoidanceModule : public SceneModuleInterface bool arrived_path_end_{false}; + bool safe_{true}; + std::shared_ptr parameters_; helper::avoidance::AvoidanceHelper helper_; @@ -576,6 +578,8 @@ class AvoidanceModule : public SceneModuleInterface ObjectDataArray registered_objects_; + mutable size_t safe_count_{0}; + mutable ObjectDataArray ego_stopped_objects_; mutable ObjectDataArray stopped_objects_; 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 1f47b804e185b..5484ef834a177 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 @@ -194,7 +194,8 @@ struct AvoidanceParameters double safety_check_backward_distance; // transit hysteresis (unsafe to safe) - double safety_check_hysteresis_factor; + size_t hysteresis_factor_safe_count; + double hysteresis_factor_expand_rate; // keep target velocity in yield maneuver double yield_velocity; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index f9903b24f42cb..79947d826ed10 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -101,7 +101,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - CollisionCheckDebug & debug); + const double hysteresis_factor, CollisionCheckDebug & debug); /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and 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 ff5706c34bdc7..e0c79a46d731e 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 @@ -1869,6 +1869,8 @@ bool AvoidanceModule::isSafePath( return true; } + const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( avoid_data_, planner_data_, parameters_, is_right_shift.value()); @@ -1879,13 +1881,16 @@ bool AvoidanceModule::isSafePath( CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - collision)) { + hysteresis_factor, collision)) { + safe_count_ = 0; return false; } } } - return true; + safe_count_++; + + return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const @@ -2505,6 +2510,8 @@ void AvoidanceModule::updateData() // update rtc status. updateRTCData(); + + safe_ = avoid_data_.safe; } void AvoidanceModule::processOnEntry() 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 c13c237673625..2d4a6b9c42b98 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -145,8 +145,9 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); + p.hysteresis_factor_expand_rate = + get_parameter(node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = get_parameter(node, ns + "hysteresis_factor_safe_count"); } // safety check rss params diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index f392ef6a26e22..31aab9da87e2a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -288,8 +288,8 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // safety check { std::string ns = "avoidance.safety_check."; - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); + p.hysteresis_factor_expand_rate = + get_parameter(node, ns + "hysteresis_factor_expand_rate"); } avoidance_parameters_ = std::make_shared(p); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 3db3e049c3c81..251720f4202d5 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1380,7 +1380,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 4cbfebef07a39..c1a883d7a9b82 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -771,7 +771,7 @@ void fillAvoidanceNecessity( } // TRUE -> ? (check with hysteresis factor) - object_data.avoid_required = check_necessity(parameters->safety_check_hysteresis_factor); + object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } void fillObjectStoppableJudge( diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 7f3fe46c6b8ec..579862de93865 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -232,7 +232,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - CollisionCheckDebug & debug) + double hysteresis_factor, CollisionCheckDebug & debug) { debug.lerped_path.reserve(target_object_path.path.size()); @@ -286,8 +286,8 @@ bool checkCollision( const auto min_lon_length = calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); - const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = rss_parameters.lateral_distance_max_threshold; + const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug)