From 7aa93149fd79659ec16a62c5cca9734096dfae53 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 1 Dec 2022 17:53:52 +0900 Subject: [PATCH] fix(obstacle_avoidance_planner): add guard for failed optimized trajectory (#2398) * fix(obstacle_avoidance_planner): add guard for failed optimized trajectory Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * add error message Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../obstacle_avoidance_planner/src/mpt_optimizer.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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());