Skip to content

Commit

Permalink
fix(osqp_interface): memory leak problem (#333)
Browse files Browse the repository at this point in the history 
* fix(osqp_interface): memory leak problem

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix: typo

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
  • Loading branch information
@rej55
rej55 authored Feb 4, 2022
1 parent 203ace1 commit f17693d
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 25 deletions.
7 changes: 4 additions & 3 deletions common/osqp_interface/include/osqp_interface/osqp_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ using autoware::common::types::float64_t;
class OSQP_INTERFACE_PUBLIC OSQPInterface
{
private:
OSQPWorkspace * m_work = nullptr;
std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace *)>> m_work;
std::unique_ptr<OSQPSettings> m_settings;
std::unique_ptr<OSQPData> m_data;
// store last work info since work is cleaned up at every execution to prevent memory leak.
Expand All @@ -59,6 +59,8 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
// Runs the solver on the stored problem.
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> solve();

static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;

public:
/// \brief Constructor without problem formulation
explicit OSQPInterface(
Expand All @@ -73,12 +75,11 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
OSQPInterface(
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u, const c_float eps_abs);
~OSQPInterface();

/****************
* OPTIMIZATION
****************/
/// \brief Solves the stored convec quadratic program (QP) problem using the OSQP solver.
/// \brief Solves the stored convex quadratic program (QP) problem using the OSQP solver.
//
/// \return The function returns a tuple containing the solution as two float vectors.
/// \return The first element of the tuple contains the 'primal' solution.
Expand Down
49 changes: 27 additions & 22 deletions common/osqp_interface/src/osqp_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ namespace common
namespace osqp
{
OSQPInterface::OSQPInterface(const c_float eps_abs, const bool8_t polish)
: m_work{nullptr, OSQPWorkspaceDeleter}
{
m_settings = std::make_unique<OSQPSettings>();
m_data = std::make_unique<OSQPData>();
Expand Down Expand Up @@ -59,6 +60,13 @@ OSQPInterface::OSQPInterface(
initializeProblem(P, A, q, l, u);
}

void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept
{
if (ptr != nullptr) {
osqp_cleanup(ptr);
}
}

void OSQPInterface::updateP(const Eigen::MatrixXd & P_new)
{
/*
Expand All @@ -71,7 +79,7 @@ void OSQPInterface::updateP(const Eigen::MatrixXd & P_new)
c_int P_elem_N = P_sparse.nonZeros();
*/
CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new);
osqp_update_P(m_work, P_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(P_csc.m_vals.size()));
osqp_update_P(m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(P_csc.m_vals.size()));
}

void OSQPInterface::updateA(const Eigen::MatrixXd & A_new)
Expand All @@ -84,29 +92,29 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new)
c_int A_elem_N = A_sparse.nonZeros();
*/
CSC_Matrix A_csc = calCSCMatrix(A_new);
osqp_update_A(m_work, A_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(A_csc.m_vals.size()));
osqp_update_A(m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(A_csc.m_vals.size()));
return;
}

void OSQPInterface::updateQ(const std::vector<double> & q_new)
{
std::vector<double> q_tmp(q_new.begin(), q_new.end());
double * q_dyn = q_tmp.data();
osqp_update_lin_cost(m_work, q_dyn);
osqp_update_lin_cost(m_work.get(), q_dyn);
}

void OSQPInterface::updateL(const std::vector<double> & l_new)
{
std::vector<double> l_tmp(l_new.begin(), l_new.end());
double * l_dyn = l_tmp.data();
osqp_update_lower_bound(m_work, l_dyn);
osqp_update_lower_bound(m_work.get(), l_dyn);
}

void OSQPInterface::updateU(const std::vector<double> & u_new)
{
std::vector<double> u_tmp(u_new.begin(), u_new.end());
double * u_dyn = u_tmp.data();
osqp_update_upper_bound(m_work, u_dyn);
osqp_update_upper_bound(m_work.get(), u_dyn);
}

void OSQPInterface::updateBounds(
Expand All @@ -116,38 +124,38 @@ void OSQPInterface::updateBounds(
std::vector<double> u_tmp(u_new.begin(), u_new.end());
double * l_dyn = l_tmp.data();
double * u_dyn = u_tmp.data();
osqp_update_bounds(m_work, l_dyn, u_dyn);
osqp_update_bounds(m_work.get(), l_dyn, u_dyn);
}

void OSQPInterface::updateEpsAbs(const double eps_abs)
{
m_settings->eps_abs = eps_abs; // for default setting
if (m_work_initialized) {
osqp_update_eps_abs(m_work, eps_abs); // for current work
osqp_update_eps_abs(m_work.get(), eps_abs); // for current work
}
}

void OSQPInterface::updateEpsRel(const double eps_rel)
{
m_settings->eps_rel = eps_rel; // for default setting
if (m_work_initialized) {
osqp_update_eps_rel(m_work, eps_rel); // for current work
osqp_update_eps_rel(m_work.get(), eps_rel); // for current work
}
}

void OSQPInterface::updateMaxIter(const int max_iter)
{
m_settings->max_iter = max_iter; // for default setting
if (m_work_initialized) {
osqp_update_max_iter(m_work, max_iter); // for current work
osqp_update_max_iter(m_work.get(), max_iter); // for current work
}
}

void OSQPInterface::updateVerbose(const bool is_verbose)
{
m_settings->verbose = is_verbose; // for default setting
if (m_work_initialized) {
osqp_update_verbose(m_work, is_verbose); // for current work
osqp_update_verbose(m_work.get(), is_verbose); // for current work
}
}

Expand All @@ -160,15 +168,15 @@ void OSQPInterface::updateRho(const double rho)
{
m_settings->rho = rho;
if (m_work_initialized) {
osqp_update_rho(m_work, rho);
osqp_update_rho(m_work.get(), rho);
}
}

void OSQPInterface::updateAlpha(const double alpha)
{
m_settings->alpha = alpha;
if (m_work_initialized) {
osqp_update_alpha(m_work, alpha);
osqp_update_alpha(m_work.get(), alpha);
}
}

Expand Down Expand Up @@ -213,24 +221,18 @@ int64_t OSQPInterface::initializeProblem(
m_data->u = u_dyn;

// Setup workspace
m_exitflag = osqp_setup(&m_work, m_data.get(), m_settings.get());
OSQPWorkspace * workspace;
m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get());
m_work.reset(workspace);
m_work_initialized = true;

return m_exitflag;
}

OSQPInterface::~OSQPInterface()
{
// Cleanup dynamic OSQP memory
if (m_work) {
osqp_cleanup(m_work);
}
}

std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> OSQPInterface::solve()
{
// Solve Problem
osqp_solve(m_work);
osqp_solve(m_work.get());

/********************
* EXTRACT SOLUTION
Expand Down Expand Up @@ -271,6 +273,9 @@ OSQPInterface::optimize(
// Run the solver on the stored problem representation.
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t> result = solve();

m_work.reset();
m_work_initialized = false;

return result;
}

Expand Down

0 comments on commit f17693d

Please sign in to comment.