add test for new functions

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

common/osqp_interface/test/test_osqp_interface.cpp

@@ -26,6 +26,10 @@ using autoware::common::osqp::float64_t;
 /// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py
 // cppcheck-suppress syntaxError
 TEST(TestOsqpInterface, BasicQp) {
+  using autoware::common::osqp::CSC_Matrix;
+  using autoware::common::osqp::calCSCMatrix;
+  using autoware::common::osqp::calCSCMatrixTrapezoidal;
+
   auto check_result =
     [](const std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> & result) {
       EXPECT_EQ(std::get<2>(result), 1);
@@ -87,5 +91,46 @@ TEST(TestOsqpInterface, BasicQp) {
     result = osqp.optimize();
     check_result(result);
   }
+  {
+    // Define problem during initialization with csc matrix
+    Eigen::MatrixXd P(2, 2);
+    P << 4, 1, 1, 2;
+    CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
+    Eigen::MatrixXd A(4, 2);
+    A << 1, 1, 1, 0, 0, 1, 0, 1;
+    CSC_Matrix A_csc = calCSCMatrix(A);
+    std::vector<float64_t> q = {1.0, 1.0};
+    std::vector<float64_t> l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF};
+    std::vector<float64_t> u = {1.0, 0.7, 0.7, autoware::common::osqp::INF};
+    autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6);
+    std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result = osqp.optimize();
+    check_result(result);
+  }
+  {
+    std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result;
+    // Dummy initial problem with csc matrix
+    Eigen::MatrixXd P = Eigen::MatrixXd::Zero(2, 2);
+    CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
+    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(4, 2);
+    CSC_Matrix A_csc = calCSCMatrix(A);
+    std::vector<float64_t> q(2, 0.0);
+    std::vector<float64_t> l(4, 0.0);
+    std::vector<float64_t> u(4, 0.0);
+    autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6);
+    osqp.optimize();
+    // Redefine problem before optimization
+    Eigen::MatrixXd P_new(2, 2);
+    P_new << 4, 1, 1, 2;
+    CSC_Matrix P_new_csc = calCSCMatrixTrapezoidal(P_new);
+    Eigen::MatrixXd A_new(4, 2);
+    A_new << 1, 1, 1, 0, 0, 1, 0, 1;
+    CSC_Matrix A_new_csc = calCSCMatrix(A_new);
+    std::vector<float64_t> q_new = {1.0, 1.0};
+    std::vector<float64_t> l_new = {1.0, 0.0, 0.0, -autoware::common::osqp::INF};
+    std::vector<float64_t> u_new = {1.0, 0.7, 0.7, autoware::common::osqp::INF};
+    osqp.initializeProblem(P_new_csc, A_new_csc, q_new, l_new, u_new);
+    result = osqp.optimize();
+    check_result(result);
+  }
 }
 }  // namespace