From 65e8c4544c85bdff16ba437dc2a2689c4cb1a21f Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 9 Aug 2022 14:13:13 +0900 Subject: [PATCH] fix(behavior_path_planner): fix not to make a large jerk avoidance path (#1540) Signed-off-by: Takamasa Horibe --- .../src/scene_module/avoidance/avoidance_module.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) 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 81624a22a17dd..6872d21fd9c46 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 @@ -2347,11 +2347,6 @@ boost::optional AvoidanceModule::findNewShiftPoint( // continue; // } - if (calcJerk(candidate) > parameters_.max_lateral_jerk) { - DEBUG_PRINT("%s, this shift exceeds jerk limit (%f). skip.", pfx, calcJerk(candidate)); - continue; - } - const auto current_shift = prev_linear_shift_path_.shift_length.at( findNearestIndex(prev_reference_.points, candidate.end.position)); @@ -2360,6 +2355,12 @@ boost::optional AvoidanceModule::findNewShiftPoint( const auto new_point_threshold = parameters_.avoidance_execution_lateral_threshold; if (std::abs(candidate.length - current_shift) > new_point_threshold) { + if (calcJerk(candidate) > parameters_.max_lateral_jerk) { + DEBUG_PRINT( + "%s, Failed to find new shift: jerk limit over (%f).", pfx, calcJerk(candidate)); + break; + } + DEBUG_PRINT( "%s, New shift point is found!!! shift change: %f -> %f", pfx, current_shift, candidate.length);