diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp index cfad1e2e55db7..f126ba9329d3e 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp @@ -189,8 +189,6 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface inline double getObjVal() const { return m_latest_work_info.obj_val; } /// \brief Returns flag asserting interface condition (Healthy condition: 0). inline int64_t getExitFlag() const { return m_exitflag; } - /// \brief Get the latest work info - inline OSQPInfo getOSQPInfo() const { return m_latest_work_info; } void logUnsolvedStatus(const std::string & prefix_message = "") const; }; diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp index 958e2e2bbded9..b7ddc79067109 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/osqp_interface/src/osqp_interface.cpp @@ -425,6 +425,12 @@ OSQPInterface::optimize( return result; } +std::tuple OSQPInterface::getSolverStatus() const +{ + return std::make_tuple( + m_latest_work_info.iter, m_latest_work_info.obj_val, m_latest_work_info.run_time); +} + void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const { const int status = getStatus(); @@ -446,13 +452,6 @@ void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); } -void OSQPInterface::getSolverStatus( - const int64_t & iter, const double & obj_val, const double & run_time) const -{ - status_iter = m_latest_work_info.iter; - status_obj_val = m_latest_work_info.obj_val; - status_run_time = m_latest_work_info.run_time; -} } // namespace osqp } // namespace common } // namespace autoware diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp index db5a3c873a5ea..ef2c3bd17c89e 100644 --- a/common/qp_interface/include/qp_interface/osqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -108,8 +108,6 @@ class OSQPInterface : public QPInterface inline double getObjVal() const { return m_latest_work_info.obj_val; } /// \brief Returns flag asserting interface condition (Healthy condition: 0). inline int64_t getExitFlag() const { return m_exitflag; } - /// \brief Get the latest work info - inline OSQPInfo getOSQPInfo() const { return m_latest_work_info; } void logUnsolvedStatus(const std::string & prefix_message = "") const; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index e7aa644ad63f9..bb38f8d5569b2 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -45,6 +45,10 @@ class QPSolverInterface const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0; + + virtual int64_t getTakenIter() const = 0; + virtual double getRunTime() const = 0; + virtual double getObjVal() const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 96c619af77497..2611f94da324f 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -53,10 +53,9 @@ class QPSolverOSQP : public QPSolverInterface const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; - int64_t getTakenIter() const { return osqpsolver_.getTakenIter(); } - double getRunTime() const { return osqpsolver_.getRunTime(); } - double getObjVal() const { return osqpsolver_.getObjVal(); } - OSQPInfo getOSQPInfo() const { return osqpsolver_.getOSQPInfo(); } + int64_t getTakenIter() const override { return osqpsolver_.getTakenIter(); } + double getRunTime() const override { return osqpsolver_.getRunTime(); } + double getObjVal() const override { return osqpsolver_.getObjVal(); } private: autoware::common::osqp::OSQPInterface osqpsolver_; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index 3a6bd2b25832b..02690715b0a83 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -16,6 +16,7 @@ #define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "osqp_interface/osqp_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { @@ -53,6 +54,13 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; + + int64_t getTakenIter() const override { return osqpsolver_.getTakenIter(); } + double getRunTime() const override { return osqpsolver_.getRunTime(); } + double getObjVal() const override { return osqpsolver_.getObjVal(); } + +private: + autoware::common::osqp::OSQPInterface osqpsolver_; }; } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index d463c3a7bfab9..f4323fc2c167b 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -147,7 +147,9 @@ Float32MultiArrayStamped MPC::generateDiagData( const double wz_predicted = current_velocity * std::tan(mpc_data.predicted_steer) / wb; const double wz_measured = current_velocity * std::tan(mpc_data.steer) / wb; const double wz_command = current_velocity * std::tan(ctrl_cmd.steering_tire_angle) / wb; - // [[maybe_unused]] const auto a = m_qpsolver_ptr->getOSQPInfo(); + const int64_t a = m_qpsolver_ptr->getTakenIter(); + + std::cerr << "iterration_number: " << a << std::endl; typedef decltype(diagnostic.data)::value_type DiagnosticValueType; const auto append_diag = [&](const auto & val) -> void {