From bab20edb5a7419910eb168becc86a5c8552666e2 Mon Sep 17 00:00:00 2001
From: Arjun Jagdish Ram <arjun.ram@tier4.jp>
Date: Mon, 17 Mar 2025 14:14:28 +0900
Subject: [PATCH 1/2] fix
Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>
---
.../design/osqp_interface-design.md | 2 +-
.../osqp_interface/csc_matrix_conv.hpp | 38 +++++++++++++
.../osqp_interface/osqp_interface.hpp | 21 ++++++--
.../src/osqp_interface.cpp | 25 +++++----
.../test/test_osqp_interface.cpp | 53 +++++++++----------
5 files changed, 96 insertions(+), 43 deletions(-)
diff --git a/common/autoware_osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md
index 887a4b4d9a..31dd5625b6 100644
--- a/common/autoware_osqp_interface/design/osqp_interface-design.md
+++ b/common/autoware_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 = std::get<0>(result);
+ std::vector<double> param = result.primal_solution;
double x_0 = param[0];
double x_1 = param[1];
```
diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp
index 8c21553152..d13d8ccb94 100644
--- a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp
+++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp
@@ -33,6 +33,44 @@ 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
diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp
index ff3013bd61..60a8202e3c 100644
--- a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp
+++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp
@@ -32,6 +32,17 @@ namespace autoware::osqp_interface
{
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.
*
@@ -52,7 +63,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
int64_t m_exitflag;
// Runs the solver on the stored problem.
- std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> solve();
+ OSQPResult solve();
static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;
@@ -93,10 +104,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 = std::get<0>(result);
+ /// \details std::vector<float> param = result.primal_solution;
/// \details double x_0 = param[0];
/// \details double x_1 = param[1];
- std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> optimize();
+ OSQPResult optimize();
/// \brief Solves convex quadratic programs (QPs) using the OSQP solver.
/// \return The function returns a tuple containing the solution as two float vectors.
@@ -111,10 +122,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 = std::get<0>(result);
+ /// \details std::vector<float> param = result.primal_solution;
/// \details double x_0 = param[0];
/// \details double x_1 = param[1];
- std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> optimize(
+ OSQPResult optimize(
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
const std::vector<double> & l, const std::vector<double> & u);
diff --git a/common/autoware_osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp
index ceeae62622..c528c20012 100644
--- a/common/autoware_osqp_interface/src/osqp_interface.cpp
+++ b/common/autoware_osqp_interface/src/osqp_interface.cpp
@@ -359,12 +359,12 @@ int64_t OSQPInterface::initializeProblem(
return m_exitflag;
}
-std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
+OSQPResult
OSQPInterface::solve()
{
// Solve Problem
- osqp_solve(m_work.get());
-
+ int32_t exit_flag = osqp_solve(m_work.get());
+
/********************
* EXTRACT SOLUTION
********************/
@@ -378,24 +378,29 @@ OSQPInterface::solve()
int64_t status_iteration = m_work->info->iter;
// Result tuple
- 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);
+ 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;
m_latest_work_info = *(m_work->info);
return result;
}
-std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
+OSQPResult
OSQPInterface::optimize()
{
// Run the solver on the stored problem representation.
- std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();
+ OSQPResult result = solve();
return result;
}
-std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
+OSQPResult
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)
@@ -404,7 +409,7 @@ OSQPInterface::optimize(
initializeProblem(P, A, q, l, u);
// Run the solver on the stored problem representation.
- std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();
+ OSQPResult result = solve();
m_work.reset();
m_work_initialized = false;
diff --git a/common/autoware_osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp
index 763baa1135..7305c9709a 100644
--- a/common/autoware_osqp_interface/test/test_osqp_interface.cpp
+++ b/common/autoware_osqp_interface/test/test_osqp_interface.cpp
@@ -44,25 +44,24 @@ TEST(TestOsqpInterface, BasicQp)
using autoware::osqp_interface::calCSCMatrixTrapezoidal;
using autoware::osqp_interface::CSC_Matrix;
- 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);
- };
+ auto check_result = [](const autoware::osqp_interface::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);
+ };
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();
@@ -73,7 +72,7 @@ TEST(TestOsqpInterface, BasicQp)
{
// Define problem during optimization
autoware::osqp_interface::OSQPInterface osqp;
- std::tuple<std::vector<double>, std::vector<double>, int, int, int> result =
+ autoware::osqp_interface::OSQPResult result =
osqp.optimize(P, A, q, l, u);
check_result(result);
}
@@ -81,12 +80,12 @@ TEST(TestOsqpInterface, BasicQp)
{
// Define problem during initialization
autoware::osqp_interface::OSQPInterface osqp(P, A, q, l, u, 1e-6);
- std::tuple<std::vector<double>, std::vector<double>, int, int, int> result = osqp.optimize();
+ autoware::osqp_interface::OSQPResult result = osqp.optimize();
check_result(result);
}
{
- std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
+ autoware::osqp_interface::OSQPResult result;
// Dummy initial problem
Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2);
Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2);
@@ -107,12 +106,12 @@ TEST(TestOsqpInterface, BasicQp)
CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
CSC_Matrix A_csc = calCSCMatrix(A);
autoware::osqp_interface::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6);
- std::tuple<std::vector<double>, std::vector<double>, int, int, int> result = osqp.optimize();
+ autoware::osqp_interface::OSQPResult result = osqp.optimize();
check_result(result);
}
{
- std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
+ autoware::osqp_interface::OSQPResult 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));
@@ -132,7 +131,7 @@ TEST(TestOsqpInterface, BasicQp)
// add warm startup
{
- std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
+ autoware::osqp_interface::OSQPResult 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));
@@ -150,8 +149,8 @@ TEST(TestOsqpInterface, BasicQp)
check_result(result);
osqp.updateCheckTermination(1);
- const auto primal_val = std::get<0>(result);
- const auto dual_val = std::get<1>(result);
+ const auto primal_val = result.primal_solution;
+ const auto dual_val = result.lagrange_multipliers;
for (size_t i = 0; i < primal_val.size(); ++i) {
std::cerr << primal_val.at(i) << std::endl;
}
From 8c266a15befe398827f0de10cfb19df4bf1a655d Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 17 Mar 2025 05:19:54 +0000
Subject: [PATCH 2/2] style(pre-commit): autofix
---
.../autoware/osqp_interface/csc_matrix_conv.hpp | 2 +-
.../autoware/osqp_interface/osqp_interface.hpp | 1 -
common/autoware_osqp_interface/src/osqp_interface.cpp | 11 ++++-------
.../test/test_osqp_interface.cpp | 3 +--
4 files changed, 6 insertions(+), 11 deletions(-)
diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp
index d13d8ccb94..62138933ce 100644
--- a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp
+++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp
@@ -33,7 +33,7 @@ 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";
diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp
index 60a8202e3c..31b2b1ae70 100644
--- a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp
+++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp
@@ -32,7 +32,6 @@ namespace autoware::osqp_interface
{
constexpr c_float INF = 1e30;
-
struct OSQPResult
{
std::vector<double> primal_solution;
diff --git a/common/autoware_osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp
index c528c20012..8b11ed148f 100644
--- a/common/autoware_osqp_interface/src/osqp_interface.cpp
+++ b/common/autoware_osqp_interface/src/osqp_interface.cpp
@@ -359,12 +359,11 @@ int64_t OSQPInterface::initializeProblem(
return m_exitflag;
}
-OSQPResult
-OSQPInterface::solve()
+OSQPResult OSQPInterface::solve()
{
// Solve Problem
int32_t exit_flag = osqp_solve(m_work.get());
-
+
/********************
* EXTRACT SOLUTION
********************/
@@ -392,16 +391,14 @@ OSQPInterface::solve()
return result;
}
-OSQPResult
-OSQPInterface::optimize()
+OSQPResult OSQPInterface::optimize()
{
// Run the solver on the stored problem representation.
OSQPResult result = solve();
return result;
}
-OSQPResult
-OSQPInterface::optimize(
+OSQPResult 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)
{
diff --git a/common/autoware_osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp
index 7305c9709a..fcb252d052 100644
--- a/common/autoware_osqp_interface/test/test_osqp_interface.cpp
+++ b/common/autoware_osqp_interface/test/test_osqp_interface.cpp
@@ -72,8 +72,7 @@ TEST(TestOsqpInterface, BasicQp)
{
// Define problem during optimization
autoware::osqp_interface::OSQPInterface osqp;
- autoware::osqp_interface::OSQPResult result =
- osqp.optimize(P, A, q, l, u);
+ autoware::osqp_interface::OSQPResult result = osqp.optimize(P, A, q, l, u);
check_result(result);
}