diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 96b6ca452bbcc..ad06a1a9f6a10 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -190,7 +190,8 @@ max_deviation_from_lane: 0.5 # [m] # avoidance distance parameters longitudinal: - prepare_time: 2.0 # [s] + min_prepare_time: 1.0 # [s] + max_prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] 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 c8b87c7b4759f..60cf9b1244236 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 @@ -211,7 +211,8 @@ struct AvoidanceParameters double stop_buffer{0.0}; // start avoidance after this time to avoid sudden path change - double prepare_time{0.0}; + double min_prepare_time{0.0}; + double max_prepare_time{0.0}; // Even if the vehicle speed is zero, avoidance will start after a distance of this much. double min_prepare_distance{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index b4c6230750946..e1d51122e5c1b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -91,10 +91,16 @@ class AvoidanceHelper return std::max(getEgoSpeed(), values.at(idx)); } + double getMinimumPrepareDistance() const + { + const auto & p = parameters_; + return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance); + } + double getNominalPrepareDistance() const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); + return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); } double getNominalAvoidanceDistance(const double shift_length) const 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 2ef0393fedb36..f690961ce0869 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 @@ -424,10 +424,23 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { // transit yield maneuver only when the avoidance maneuver is not initiated. if (helper_.isShifted()) { + RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated."); return false; } + // prevent sudden steering. + const auto registered_lines = path_shifter_.getShiftLines(); + if (!registered_lines.empty()) { + const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + const auto to_shift_start_point = calcSignedArcLength( + path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); + if (to_shift_start_point < helper_.getMinimumPrepareDistance()) { + return false; + } + } + if (!data.stop_target_object) { + RCLCPP_DEBUG(getLogger(), "can pass by the object safely without avoidance maneuver."); return true; } @@ -1640,6 +1653,10 @@ void AvoidanceModule::applySmallShiftFilter( continue; } + if (s.start_longitudinal < helper_.getMinimumPrepareDistance()) { + continue; + } + shift_lines.push_back(s); } } @@ -2393,9 +2410,8 @@ AvoidLineArray AvoidanceModule::findNewShiftLine( for (size_t i = 0; i < shift_lines.size(); ++i) { const auto & candidate = shift_lines.at(i); - // new shift points must exist in front of Ego - // this value should be larger than -eps consider path shifter calculation error. - if (candidate.start_idx < avoid_data_.ego_closest_path_index) { + // prevent sudden steering. + if (candidate.start_longitudinal < helper_.getMinimumPrepareDistance()) { break; } 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 6ed67fc817f28..f71024dcc5034 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -221,7 +221,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( // avoidance maneuver (longitudinal) { std::string ns = "avoidance.avoidance.longitudinal."; - p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); @@ -390,7 +391,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "prepare_time", p->prepare_time); + updateParam(parameters, ns + "min_prepare_time", p->min_prepare_time); + updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); }