diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 5171dfddc88e6..439b68e9032dd 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -921,7 +921,7 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin // Only the Lower bound is cut out. Widen the bounds towards the lower bound since cut out too // much. b.lower_bound = - std::min(b.lower_bound, original_b.upper_bound + mpt_param_.min_drivable_width); + std::min(b.lower_bound, original_b.upper_bound - mpt_param_.min_drivable_width); continue; } // extend longitudinal if it overlaps out_of_upper_bound_sections @@ -971,7 +971,7 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin // Only the Upper bound is cut out. Widen the bounds towards the upper bound since cut out too // much. b.upper_bound = - std::max(b.upper_bound, original_b.lower_bound - mpt_param_.min_drivable_width); + std::max(b.upper_bound, original_b.lower_bound + mpt_param_.min_drivable_width); continue; } // extend longitudinal if it overlaps out_of_lower_bound_sections