refactor(mpc_lateral_controller): add debug info of qp solver (autowarefoundation#5459) (#1098)

* add debug info of qp solver



* no info for EigenLeastSquareLLT



* return 0 in base class

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: 3 people

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_

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_;

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_

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;
 }