From 0b33cd18b2f3e139165001091e0edc6ddd5aa561 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 14 May 2023 20:12:30 +0900 Subject: [PATCH] fix(obstacle_avoidance_planner): fix optimization constraint for narrow-road driving (#3701) Signed-off-by: Takayuki Murooka --- planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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