refactor(mpc_lateral_controller): add debug info of qp solver (autowa…

…refoundation#5459) (#1098) * add debug info of qp solver * no info for EigenLeastSquareLLT * return 0 in base class --------- Signed-off-by: kyoichi-sugahara <[email protected]> Co-authored-by: Kyoichi Sugahara <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Feb 2, 2024 · 4f09ac5 · 4f09ac5
1 parent 756a818
commit 4f09ac5
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 0 deletions.
diff --git a/...l/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/...l/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
@@ -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/...teral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp b/...teral_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
@@ -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;
 }