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

…ctory (autowarefoundation#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>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

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());