From 5ad71564cd11702567398ecd96cd735a0f529c6e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 10 May 2022 17:39:28 +0900 Subject: [PATCH] fix(avoidance_module): ignore object instead of creating zero shift (#731) * fix: ignore object instead of creating zero shift instead of creating zero shift point, the object will be ignored. no behavior changes should be observed. Signed-off-by: Muhammad Zulfaqar Azmi * refactor: sync continue with upstream Signed-off-by: Muhammad Zulfaqar Azmi * fix: fix debug message for insufficient lateral margin Signed-off-by: Muhammad Zulfaqar Azmi --- .../avoidance/avoidance_module.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 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 876e5fc3112c3..2923996ca8697 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 @@ -490,24 +490,19 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( avoidance_debug_msg.max_shift_length = max_allowable_lateral_distance; if (!(o.to_road_shoulder_distance > max_allowable_lateral_distance)) { - avoidance_debug_msg.allow_avoidance = false; - avoidance_debug_msg.failed_reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN); + continue; } const auto max_shift_length = o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width; - const auto max_left_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length, - this]() noexcept { - const auto left_shift_constraint = std::min(getLeftShiftBound(), max_shift_length); - return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? left_shift_constraint - : 0.0; + + const auto max_left_shift_limit = [&max_shift_length, this]() noexcept { + return std::min(getLeftShiftBound(), max_shift_length); }; - const auto max_right_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length, - this]() noexcept { - const auto right_shift_constraint = std::max(getRightShiftBound(), -max_shift_length); - return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? right_shift_constraint - : 0.0; + const auto max_right_shift_limit = [&max_shift_length, this]() noexcept { + return std::max(getRightShiftBound(), -max_shift_length); }; const auto shift_length = isOnRight(o)