Revert "feat(path_optimizer): additional failure logging and failure mode han…"

This reverts commit 30880cc.

Author: PanConChicharron

common/osqp_interface/design/osqp_interface-design.md

@@ -54,7 +54,7 @@ The interface can be used in several ways:
 
    ```cpp
    std::tuple<std::vector<double>, std::vector<double>> result = osqp_interface.optimize();
-   std::vector<double> param = result.primal_solution;
+   std::vector<double> param = std::get<0>(result);
    double x_0 = param[0];
    double x_1 = param[1];
    ```

common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp

@@ -37,44 +37,6 @@ struct OSQP_INTERFACE_PUBLIC CSC_Matrix
   std::vector<c_int> m_row_idxs;
   /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer')
   std::vector<c_int> m_col_idxs;
-
-  friend std::ostream & operator<<(std::ostream & os, const CSC_Matrix & matrix)
-  {
-    os << "CSC_Matrix: {\n";
-    os << "\tm_vals: [";
-
-    // Iterator-based loop for m_vals
-    for (auto it = std::begin(matrix.m_vals); it != std::end(matrix.m_vals); ++it) {
-      os << *it;  // Print the current element (dereference iterator)
-      if (std::next(it) != std::end(matrix.m_vals)) {  // Check if not the last element
-        os << ", ";
-      }
-    }
-    os << "],\n";
-
-    os << "\tm_row_idxs: [";
-    // Iterator-based loop for m_row_idxs
-    for (auto it = std::begin(matrix.m_row_idxs); it != std::end(matrix.m_row_idxs); ++it) {
-      os << *it;
-      if (std::next(it) != std::end(matrix.m_row_idxs)) {
-        os << ", ";
-      }
-    }
-    os << "],\n";
-
-    os << "\tm_col_idxs: [";
-    // Iterator-based loop for m_col_idxs
-    for (auto it = std::begin(matrix.m_col_idxs); it != std::end(matrix.m_col_idxs); ++it) {
-      os << *it;
-      if (std::next(it) != std::end(matrix.m_col_idxs)) {
-        os << ", ";
-      }
-    }
-    os << "]\n";
-
-    os << "}\n";
-    return os;
-  }
 };
 
 /// \brief Calculate CSC matrix from Eigen matrix

common/osqp_interface/include/osqp_interface/osqp_interface.hpp

@@ -36,16 +36,6 @@ namespace osqp
 {
 constexpr c_float INF = 1e30;
 
-struct OSQPResult
-{
-  std::vector<double> primal_solution;
-  std::vector<double> lagrange_multipliers;
-  int polish_status;
-  int solution_status;
-  int iteration_status;
-  int exit_flag;
-};
-
 /**
  * Implementation of a native C++ interface for the OSQP solver.
  *
@@ -66,7 +56,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
   int64_t m_exitflag;
 
   // Runs the solver on the stored problem.
-  OSQPResult solve();
+  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> solve();
 
   static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;
 
@@ -107,10 +97,10 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
   /// \details        std::tuple<std::vector<double>, std::vector<double>> result;
   /// \details        result = osqp_interface.optimize();
   /// \details   4. Access the optimized parameters.
-  /// \details        std::vector<float> param = result.primal_solution;
+  /// \details        std::vector<float> param = std::get<0>(result);
   /// \details        double x_0 = param[0];
   /// \details        double x_1 = param[1];
-  OSQPResult optimize();
+  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> optimize();
 
   /// \brief Solves convex quadratic programs (QPs) using the OSQP solver.
   /// \return The function returns a tuple containing the solution as two float vectors.
@@ -125,10 +115,10 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
   /// \details        std::tuple<std::vector<double>, std::vector<double>> result;
   /// \details        result = osqp_interface.optimize(P, A, q, l, u, 1e-6);
   /// \details   4. Access the optimized parameters.
-  /// \details        std::vector<float> param = result.primal_solution;
+  /// \details        std::vector<float> param = std::get<0>(result);
   /// \details        double x_0 = param[0];
   /// \details        double x_1 = param[1];
-  OSQPResult optimize(
+  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> optimize(
     const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
     const std::vector<double> & l, const std::vector<double> & u);
 

common/osqp_interface/src/osqp_interface.cpp

@@ -363,10 +363,11 @@ int64_t OSQPInterface::initializeProblem(
   return m_exitflag;
 }
 
-OSQPResult OSQPInterface::solve()
+std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
+OSQPInterface::solve()
 {
   // Solve Problem
-  int32_t exit_flag = osqp_solve(m_work.get());
+  osqp_solve(m_work.get());
 
   /********************
    * EXTRACT SOLUTION
@@ -381,36 +382,33 @@ OSQPResult OSQPInterface::solve()
   int64_t status_iteration = m_work->info->iter;
 
   // Result tuple
-  OSQPResult result;
-
-  result.primal_solution = sol_primal;
-  result.lagrange_multipliers = sol_lagrange_multiplier;
-  result.polish_status = status_polish;
-  result.solution_status = status_solution;
-  result.iteration_status = status_iteration;
-  result.exit_flag = exit_flag;
+  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result =
+    std::make_tuple(
+      sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration);
 
   m_latest_work_info = *(m_work->info);
 
   return result;
 }
 
-OSQPResult OSQPInterface::optimize()
+std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
+OSQPInterface::optimize()
 {
   // Run the solver on the stored problem representation.
-  OSQPResult result = solve();
+  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();
   return result;
 }
 
-OSQPResult OSQPInterface::optimize(
+std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
+OSQPInterface::optimize(
   const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
   const std::vector<double> & l, const std::vector<double> & u)
 {
   // Allocate memory for problem
   initializeProblem(P, A, q, l, u);
 
   // Run the solver on the stored problem representation.
-  OSQPResult result = solve();
+  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();
 
   m_work.reset();
   m_work_initialized = false;

common/osqp_interface/test/test_osqp_interface.cpp

@@ -44,24 +44,25 @@ TEST(TestOsqpInterface, BasicQp)
   using autoware::common::osqp::calCSCMatrixTrapezoidal;
   using autoware::common::osqp::CSC_Matrix;
 
-  auto check_result = [](const autoware::common::osqp::OSQPResult & result) {
-    EXPECT_EQ(result.polish_status, 1);    // polish succeeded
-    EXPECT_EQ(result.solution_status, 1);  // solution succeeded
-
-    static const auto ep = 1.0e-8;
-
-    const auto prime_val = result.primal_solution;
-    ASSERT_EQ(prime_val.size(), size_t(2));
-    EXPECT_NEAR(prime_val[0], 0.3, ep);
-    EXPECT_NEAR(prime_val[1], 0.7, ep);
-
-    const auto dual_val = result.lagrange_multipliers;
-    ASSERT_EQ(dual_val.size(), size_t(4));
-    EXPECT_NEAR(dual_val[0], -2.9, ep);
-    EXPECT_NEAR(dual_val[1], 0.0, ep);
-    EXPECT_NEAR(dual_val[2], 0.2, ep);
-    EXPECT_NEAR(dual_val[3], 0.0, ep);
-  };
+  auto check_result =
+    [](const std::tuple<std::vector<double>, std::vector<double>, int, int, int> & result) {
+      EXPECT_EQ(std::get<2>(result), 1);  // polish succeeded
+      EXPECT_EQ(std::get<3>(result), 1);  // solution succeeded
+
+      static const auto ep = 1.0e-8;
+
+      const auto prime_val = std::get<0>(result);
+      ASSERT_EQ(prime_val.size(), size_t(2));
+      EXPECT_NEAR(prime_val[0], 0.3, ep);
+      EXPECT_NEAR(prime_val[1], 0.7, ep);
+
+      const auto dual_val = std::get<1>(result);
+      ASSERT_EQ(dual_val.size(), size_t(4));
+      EXPECT_NEAR(dual_val[0], -2.9, ep);
+      EXPECT_NEAR(dual_val[1], 0.0, ep);
+      EXPECT_NEAR(dual_val[2], 0.2, ep);
+      EXPECT_NEAR(dual_val[3], 0.0, ep);
+    };
 
   const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished();
   const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished();
@@ -72,19 +73,20 @@ TEST(TestOsqpInterface, BasicQp)
   {
     // Define problem during optimization
     autoware::common::osqp::OSQPInterface osqp;
-    autoware::common::osqp::OSQPResult result = osqp.optimize(P, A, q, l, u);
+    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result =
+      osqp.optimize(P, A, q, l, u);
     check_result(result);
   }
 
   {
     // Define problem during initialization
     autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6);
-    autoware::common::osqp::OSQPResult result = osqp.optimize();
+    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result = osqp.optimize();
     check_result(result);
   }
 
   {
-    autoware::common::osqp::OSQPResult result;
+    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
     // Dummy initial problem
     Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2);
     Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2);
@@ -105,12 +107,12 @@ TEST(TestOsqpInterface, BasicQp)
     CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
     CSC_Matrix A_csc = calCSCMatrix(A);
     autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6);
-    autoware::common::osqp::OSQPResult result = osqp.optimize();
+    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result = osqp.optimize();
     check_result(result);
   }
 
   {
-    autoware::common::osqp::OSQPResult result;
+    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
     // Dummy initial problem with csc matrix
     CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2));
     CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2));
@@ -130,7 +132,7 @@ TEST(TestOsqpInterface, BasicQp)
 
   // add warm startup
   {
-    autoware::common::osqp::OSQPResult result;
+    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
     // Dummy initial problem with csc matrix
     CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2));
     CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2));
@@ -148,8 +150,8 @@ TEST(TestOsqpInterface, BasicQp)
     check_result(result);
 
     osqp.updateCheckTermination(1);
-    const auto primal_val = result.primal_solution;
-    const auto dual_val = result.lagrange_multipliers;
+    const auto primal_val = std::get<0>(result);
+    const auto dual_val = std::get<1>(result);
     for (size_t i = 0; i < primal_val.size(); ++i) {
       std::cerr << primal_val.at(i) << std::endl;
     }

control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp

@@ -54,11 +54,11 @@ bool QPSolverOSQP::solve(
   /* execute optimization */
   auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound);
 
-  std::vector<double> U_osqp = result.primal_solution;
+  std::vector<double> U_osqp = std::get<0>(result);
   u = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
     &U_osqp[0], static_cast<Eigen::Index>(U_osqp.size()), 1);
 
-  const int status_val = result.solution_status;
+  const int status_val = std::get<3>(result);
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
     return false;
@@ -71,7 +71,7 @@ bool QPSolverOSQP::solve(
   }
 
   // polish status: successful (1), unperformed (0), (-1) unsuccessful
-  int status_polish = result.polish_status;
+  int status_polish = std::get<2>(result);
   if (status_polish == -1 || status_polish == 0) {
     const auto s = (status_polish == 0) ? "Polish process is not performed in osqp."
                                         : "Polish process failed in osqp.";

planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp

@@ -244,11 +244,10 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   }
 
   // execute optimization
-  const autoware::common::osqp::OSQPResult result =
-    qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
-  const std::vector<double> optval = result.primal_solution;
+  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 = result.solution_status;
+  const int status_val = std::get<3>(result);
   if (status_val != 1)
     std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl;