fix(obstacle_avoidance_planner): add guard for failed optimized traje…

…ctory (#2398) * fix(obstacle_avoidance_planner): add guard for failed optimized trajectory Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add error message Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Dec 1, 2022 · 7aa9314 · 7aa9314
1 parent 17f1b95
commit 7aa9314
Showing 1 changed file with 12 additions and 0 deletions.
diff --git 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::MPTTrajs> 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());