temp

Signed-off-by: kyoichi-sugahara <[email protected]>
kyoichi-sugahara · Nov 1, 2023 · 2b4ed6d · 2b4ed6d
1 parent cd1fd00
commit 2b4ed6d
Show file tree

Hide file tree

Showing 7 changed files with 24 additions and 16 deletions.
diff --git 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
@@ -425,6 +425,12 @@ OSQPInterface::optimize(
   return result;
 }
 
+std::tuple<int64_t, double, double> 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
@@ -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/...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 = 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
@@ -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/...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
@@ -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
@@ -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 {