feat: handle NaNs when using OSQP (#3490)

Signed-off-by: Maxime CLEMENT <[email protected]>
autowarefoundation · May 8, 2023 · bc41dbe · bc41dbe
1 parent 1356149
commit bc41dbe
Show file tree

Hide file tree

Showing 7 changed files with 78 additions and 20 deletions.
diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp
@@ -60,8 +60,14 @@ bool QPSolverOSQP::solve(
 
   const int status_val = std::get<3>(result);
   if (status_val != 1) {
-    // TODO(Horibe): Should return false and the failure must be handled in an appropriate way.
     RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
+    return false;
+  }
+  const auto has_nan =
+    std::any_of(U_osqp.begin(), U_osqp.end(), [](const auto v) { return std::isnan(v); });
+  if (has_nan) {
+    RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
+    return false;
   }
 
   // polish status: successful (1), unperformed (0), (-1) unsuccessful

diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
@@ -294,6 +294,17 @@ bool JerkFilteredSmoother::apply(
   // execute optimization
   const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
   const std::vector<double> optval = std::get<0>(result);
+  const int status_val = std::get<3>(result);
+  if (status_val != 1) {
+    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
+    return false;
+  }
+  const auto has_nan =
+    std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); });
+  if (has_nan) {
+    RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
+    return false;
+  }
 
   const auto tf1 = std::chrono::system_clock::now();
   const double dt_ms1 =

diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
@@ -190,6 +190,17 @@ bool L2PseudoJerkSmoother::apply(
   // [b0, b1, ..., bN, |  a0, a1, ..., aN, |
   //  delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN]
   const std::vector<double> optval = std::get<0>(result);
+  const int status_val = std::get<3>(result);
+  if (status_val != 1) {
+    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
+    return false;
+  }
+  const auto has_nan =
+    std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); });
+  if (has_nan) {
+    RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
+    return false;
+  }
 
   for (unsigned int i = 0; i < N; ++i) {
     double v = optval.at(i);

diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
@@ -205,6 +205,17 @@ bool LinfPseudoJerkSmoother::apply(
   // [b0, b1, ..., bN, |  a0, a1, ..., aN, |
   //  delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN]
   const std::vector<double> optval = std::get<0>(result);
+  const int status_val = std::get<3>(result);
+  if (status_val != 1) {
+    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
+    return false;
+  }
+  const auto has_nan =
+    std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); });
+  if (has_nan) {
+    RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
+    return false;
+  }
 
   /* get velocity & acceleration */
   for (unsigned int i = 0; i < N; ++i) {

diff --git a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp
@@ -380,6 +380,12 @@ std::optional<std::vector<double>> EBPathSmoother::optimizeTrajectory()
     osqp_solver_ptr_->logUnsolvedStatus("[EB]");
     return std::nullopt;
   }
+  const auto has_nan = std::any_of(
+    optimized_points.begin(), optimized_points.end(), [](const auto v) { return std::isnan(v); });
+  if (has_nan) {
+    RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
+    return std::nullopt;
+  }
 
   time_keeper_ptr_->toc(__func__, "        ");
   return optimized_points;

diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
@@ -1540,6 +1540,13 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
   // get optimization result
   auto optimization_result =
     std::get<0>(result);  // NOTE: const cannot be added due to the next operation.
+  const auto has_nan = std::any_of(
+    optimization_result.begin(), optimization_result.end(),
+    [](const auto v) { return std::isnan(v); });
+  if (has_nan) {
+    RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
+    return std::nullopt;
+  }
   const Eigen::VectorXd optimized_steer_angles =
     Eigen::Map<Eigen::VectorXd>(&optimization_result[0], D_un);
 

diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp
@@ -248,29 +248,35 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   const std::vector<double> optval = std::get<0>(result);
 
   const int status_val = std::get<3>(result);
-  if (status_val != 1) {
+  if (status_val != 1)
     std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl;
-  }
 
-  std::vector<double> opt_time = time_vec;
-  std::vector<double> opt_pos(N);
-  std::vector<double> opt_vel(N);
-  std::vector<double> opt_acc(N);
-  std::vector<double> opt_jerk(N);
-  for (size_t i = 0; i < N; ++i) {
-    opt_pos.at(i) = optval.at(IDX_S0 + i);
-    opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0);
-    opt_acc.at(i) = optval.at(IDX_A0 + i);
-    opt_jerk.at(i) = optval.at(IDX_J0 + i);
-  }
-  opt_vel.back() = 0.0;
+  const auto has_nan =
+    std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); });
+  if (has_nan) std::cerr << "optimization failed : result contains NaN values\n";
 
   OptimizationResult optimized_result;
-  optimized_result.t = opt_time;
-  optimized_result.s = opt_pos;
-  optimized_result.v = opt_vel;
-  optimized_result.a = opt_acc;
-  optimized_result.j = opt_jerk;
+  const auto is_optimization_failed = status_val != 1 || has_nan;
+  if (!is_optimization_failed) {
+    std::vector<double> opt_time = time_vec;
+    std::vector<double> opt_pos(N);
+    std::vector<double> opt_vel(N);
+    std::vector<double> opt_acc(N);
+    std::vector<double> opt_jerk(N);
+    for (size_t i = 0; i < N; ++i) {
+      opt_pos.at(i) = optval.at(IDX_S0 + i);
+      opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0);
+      opt_acc.at(i) = optval.at(IDX_A0 + i);
+      opt_jerk.at(i) = optval.at(IDX_J0 + i);
+    }
+    opt_vel.back() = 0.0;
+
+    optimized_result.t = opt_time;
+    optimized_result.s = opt_pos;
+    optimized_result.v = opt_vel;
+    optimized_result.a = opt_acc;
+    optimized_result.j = opt_jerk;
+  }
 
   return optimized_result;
 }