From 9ea99f9845d418c8334fa846197ec56fd61d4a75 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 29 Nov 2022 13:55:25 +0900 Subject: [PATCH 1/3] fix(obstacle_avoidance_planner): add guard for failed optimized trajectory Signed-off-by: Takayuki Murooka --- .../obstacle_avoidance_planner/src/mpt_optimizer.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 1a8e70f9136dd..f3da6037a801f 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -296,6 +296,15 @@ 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_ptr->lateral_errors) { + if (max_lateral_deviation < std::abs(lateral_error)) { + 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()); From fb0cae02048dc30417f2987b26b18fae99afe07a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 29 Nov 2022 15:12:16 +0900 Subject: [PATCH 2/3] fix Signed-off-by: Takayuki Murooka --- planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index f3da6037a801f..c66b26a2eddd7 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -299,7 +299,7 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto // 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_ptr->lateral_errors) { + for (const double lateral_error : debug_data.lateral_errors) { if (max_lateral_deviation < std::abs(lateral_error)) { return boost::none; } From 412de4cc9109e8978f06043e0159c85f0ca1c034 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 1 Dec 2022 17:13:04 +0900 Subject: [PATCH 3/3] add error message Signed-off-by: Takayuki Murooka --- planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index c66b26a2eddd7..56d74c9eab4ef 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -301,6 +301,9 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto 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; } }