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 ef62b7c1975a6..15c6039d42d48 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 @@ -465,8 +465,8 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( const auto & lat_collision_margin = parameters_.lateral_collision_margin; const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto & road_shoulder_safety_margin = parameters_.road_shoulder_safety_margin; - const auto max_allowable_lateral_distance = - lat_collision_safety_buffer + lat_collision_margin + vehicle_width; + const auto max_allowable_lateral_distance = lat_collision_safety_buffer + lat_collision_margin + + vehicle_width - road_shoulder_safety_margin; const auto avoid_margin = lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width; @@ -494,20 +494,9 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( 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 = [&max_shift_length, this]() noexcept { - return std::min(getLeftShiftBound(), max_shift_length); - }; - - const auto max_right_shift_limit = [&max_shift_length, this]() noexcept { - return std::max(getRightShiftBound(), -max_shift_length); - }; - const auto shift_length = isOnRight(o) - ? std::min(o.overhang_dist + avoid_margin, max_left_shift_limit()) - : std::max(o.overhang_dist - avoid_margin, max_right_shift_limit()); + ? std::min(o.overhang_dist + avoid_margin, getLeftShiftBound()) + : std::max(o.overhang_dist - avoid_margin, getRightShiftBound()); const auto avoiding_shift = shift_length - current_ego_shift; const auto return_shift = shift_length;