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 3095ec2c03251..55ecee17b9778 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 @@ -480,6 +480,15 @@ class AvoidanceModule : public SceneModuleInterface */ bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const; + bool isComfortable(const AvoidLineArray & shift_lines) const + { + return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { + return PathShifter::calcJerkFromLatLonDistance( + line.getRelativeLength(), line.getRelativeLongitudinal(), + helper_.getAvoidanceEgoSpeed()) < helper_.getLateralMaxJerkLimit(); + }); + } + // post process /** 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 0263eee5fe727..9a3f73855e0f7 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 @@ -465,6 +465,8 @@ struct AvoidancePlanningData bool safe{false}; + bool comfortable{false}; + bool avoid_required{false}; bool yield_required{false}; 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 db80bc72f21ad..e3310ccf57bdd 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 @@ -129,7 +129,7 @@ bool AvoidanceModule::isExecutionRequested() const bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoidance_data_.safe; + return avoidance_data_.safe && avoidance_data_.comfortable; } bool AvoidanceModule::canTransitSuccessState() @@ -448,6 +448,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * Check avoidance path safety. For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ + data.comfortable = isComfortable(data.unapproved_new_sl); data.safe = isSafePath(data.candidate_path, debug); } @@ -2369,13 +2370,6 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat return subsequent; }; - // check jerk limit. - const auto is_large_jerk = [this](const auto & s) { - const auto jerk = PathShifter::calcJerkFromLatLonDistance( - s.getRelativeLength(), s.getRelativeLongitudinal(), helper_.getAvoidanceEgoSpeed()); - return jerk > helper_.getLateralMaxJerkLimit(); - }; - // check ignore or not. const auto is_ignore_shift = [this](const auto & s) { return std::abs(helper_.getRelativeShiftToPath(s)) < parameters_->lateral_execution_threshold; @@ -2392,10 +2386,6 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat } if (!is_ignore_shift(candidate)) { - if (is_large_jerk(candidate)) { - break; - } - return get_subsequent_shift(i); } }