From 4f09ac5df1cf9c8c0f4559045c55ba548038facb Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 1 Feb 2024 16:15:05 +0900 Subject: [PATCH] refactor(mpc_lateral_controller): add debug info of qp solver (#5459) (#1098) * add debug info of qp solver * no info for EigenLeastSquareLLT * return 0 in base class --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kyoichi Sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../qp_solver/qp_solver_interface.hpp | 4 ++++ .../mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp | 4 ++++ .../qp_solver/qp_solver_unconstraint_fast.hpp | 7 +++++++ control/mpc_lateral_controller/src/mpc.cpp | 6 ++++++ 4 files changed, 21 insertions(+) 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..ca30bd30e4dd1 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 { return 0; } + virtual double getRunTime() const { return 0.0; } + virtual double getObjVal() const { return 0.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 cdb590faab84d..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,6 +53,10 @@ 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 override { return osqpsolver_.getTakenIter(); } + double getRunTime() const override { return osqpsolver_.getRunTime(); } + double getObjVal() const override { return osqpsolver_.getObjVal(); } + private: autoware::common::osqp::OSQPInterface osqpsolver_; rclcpp::Logger logger_; 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..ef337eaaa8528 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 @@ -53,6 +53,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 1; }; + // TODO(someone): calculate runtime and object value, otherwise just return 0 by base class + // double getRunTime() const override { return 0.0; } + // double getObjVal() const override { return 0.0; } + +private: }; } // 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 8ff323727f25e..f1948f9f8e08b 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -145,6 +145,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; + const int iteration_num = m_qpsolver_ptr->getTakenIter(); + const double runtime = m_qpsolver_ptr->getRunTime(); + const double objective_value = m_qpsolver_ptr->getObjVal(); typedef decltype(diagnostic.data)::value_type DiagnosticValueType; const auto append_diag = [&](const auto & val) -> void { @@ -168,6 +171,9 @@ Float32MultiArrayStamped MPC::generateDiagData( append_diag(nearest_k); // [15] nearest path curvature (not smoothed) append_diag(mpc_data.predicted_steer); // [16] predicted steer append_diag(wz_predicted); // [17] angular velocity from predicted steer + append_diag(iteration_num); // [18] iteration number + append_diag(runtime); // [19] runtime of the latest problem solved + append_diag(objective_value); // [20] objective value of the latest problem solved return diagnostic; }