diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 1a8e70f9136dd..56d74c9eab4ef 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -296,6 +296,18 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto fixed_ref_points, non_fixed_ref_points, optimized_control_variables.get(), mpt_matrix, debug_data); + // NOTE: Sometimes optimization failed without failed status. + // Therefore, we have to check if optimization was solved correctly by the result. + constexpr double max_lateral_deviation = 3.0; + for (const double lateral_error : debug_data.lateral_errors) { + if (max_lateral_deviation < std::abs(lateral_error)) { + RCLCPP_ERROR( + rclcpp::get_logger("mpt_optimizer"), + "return boost::none since lateral deviation is too large."); + return boost::none; + } + } + auto full_optimized_ref_points = fixed_ref_points; full_optimized_ref_points.insert( full_optimized_ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end());