Merge pull request #1453 from tier4/perf/velocity-smoother

perf(velocity_smoother): decrease the processing time

common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp

@@ -21,17 +21,17 @@
 
 #include <vector>
 
-namespace qp
+namespace autoware::common
 {
 /// \brief Compressed-Column-Sparse Matrix
 struct CSC_Matrix
 {
   /// Vector of non-zero values. Ex: [4,1,1,2]
-  std::vector<c_float> m_vals;
+  std::vector<c_float> vals_;
   /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner')
-  std::vector<c_int> m_row_idxs;
+  std::vector<c_int> row_idxs_;
   /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer')
-  std::vector<c_int> m_col_idxs;
+  std::vector<c_int> col_idxs_;
 };
 
 /// \brief Calculate CSC matrix from Eigen matrix
@@ -41,6 +41,6 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat);
 /// \brief Print the given CSC matrix to the standard output
 void printCSCMatrix(const CSC_Matrix & csc_mat);
 
-}  // namespace qp
+}  // namespace autoware::common
 
 #endif  // QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_

common/qp_interface/include/qp_interface/osqp_interface.hpp

@@ -24,17 +24,19 @@
 #include <string>
 #include <vector>
 
-namespace qp
+namespace autoware::common
 {
-constexpr c_float INF = 1e30;
+constexpr c_float OSQP_INF = 1e30;
 
 class OSQPInterface : public QPInterface
 {
 public:
   /// \brief Constructor without problem formulation
   OSQPInterface(
-    const bool enable_warm_start = false,
-    const c_float eps_abs = std::numeric_limits<c_float>::epsilon(), const bool polish = true);
+    const bool enable_warm_start = false, const int max_iteration = 20000,
+    const c_float eps_abs = std::numeric_limits<c_float>::epsilon(),
+    const c_float eps_rel = std::numeric_limits<c_float>::epsilon(), const bool polish = true,
+    const bool verbose = false);
   /// \brief Constructor with problem setup
   /// \param P: (n,n) matrix defining relations between parameters.
   /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound.
@@ -60,8 +62,10 @@ class OSQPInterface : public QPInterface
     CSC_Matrix P, CSC_Matrix A, const std::vector<double> & q, const std::vector<double> & l,
     const std::vector<double> & u);
 
-  int getIteration() const override;
-  int getStatus() const override;
+  int getIterationNumber() const override;
+  bool isSolved() const override;
+  std::string getStatus() const override;
+
   int getPolishStatus() const;
   std::vector<double> getDualSolution() const;
 
@@ -96,20 +100,18 @@ class OSQPInterface : public QPInterface
   void updateCheckTermination(const int check_termination);
 
   /// \brief Get the number of iteration taken to solve the problem
-  inline int64_t getTakenIter() const { return static_cast<int64_t>(m_latest_work_info.iter); }
+  inline int64_t getTakenIter() const { return static_cast<int64_t>(latest_work_info_.iter); }
   /// \brief Get the status message for the latest problem solved
   inline std::string getStatusMessage() const
   {
-    return static_cast<std::string>(m_latest_work_info.status);
+    return static_cast<std::string>(latest_work_info_.status);
   }
   /// \brief Get the runtime of the latest problem solved
-  inline double getRunTime() const { return m_latest_work_info.run_time; }
+  inline double getRunTime() const { return latest_work_info_.run_time; }
   /// \brief Get the objective value the latest problem solved
-  inline double getObjVal() const { return m_latest_work_info.obj_val; }
+  inline double getObjVal() const { return latest_work_info_.obj_val; }
   /// \brief Returns flag asserting interface condition (Healthy condition: 0).
-  inline int64_t getExitFlag() const { return m_exitflag; }
-
-  void logUnsolvedStatus(const std::string & prefix_message = "") const;
+  inline int64_t getExitFlag() const { return exitflag_; }
 
   // Setter functions for warm start
   bool setWarmStart(
@@ -118,17 +120,17 @@ class OSQPInterface : public QPInterface
   bool setDualVariables(const std::vector<double> & dual_variables);
 
 private:
-  std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace *)>> m_work;
-  std::unique_ptr<OSQPSettings> m_settings;
-  std::unique_ptr<OSQPData> m_data;
+  std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace *)>> work_;
+  std::unique_ptr<OSQPSettings> settings_;
+  std::unique_ptr<OSQPData> data_;
   // store last work info since work is cleaned up at every execution to prevent memory leak.
-  OSQPInfo m_latest_work_info;
+  OSQPInfo latest_work_info_;
   // Number of parameters to optimize
-  int64_t m_param_n;
+  int64_t param_n_;
   // Flag to check if the current work exists
-  bool m_work_initialized = false;
+  bool work__initialized = false;
   // Exitflag
-  int64_t m_exitflag;
+  int64_t exitflag_;
 
   void initializeProblemImpl(
     const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
@@ -140,6 +142,6 @@ class OSQPInterface : public QPInterface
 
   std::vector<double> optimizeImpl() override;
 };
-}  // namespace qp
+}  // namespace autoware::common
 
 #endif  // QP_INTERFACE__OSQP_INTERFACE_HPP_

common/qp_interface/include/qp_interface/proxqp_interface.hpp

@@ -22,34 +22,36 @@
 
 #include <limits>
 #include <memory>
+#include <string>
 #include <vector>
 
-namespace qp
+namespace autoware::common
 {
 class ProxQPInterface : public QPInterface
 {
 public:
   explicit ProxQPInterface(
-    const bool enable_warm_start = false,
-    const double eps_abs = std::numeric_limits<double>::epsilon());
+    const bool enable_warm_start, const int max_iteration, const double eps_abs,
+    const double eps_rel, const bool verbose = false);
 
-  int getIteration() const override;
-  int getStatus() const override;
+  int getIterationNumber() const override;
+  bool isSolved() const override;
+  std::string getStatus() const override;
 
   void updateEpsAbs(const double eps_abs) override;
   void updateEpsRel(const double eps_rel) override;
   void updateVerbose(const bool verbose) override;
 
 private:
-  proxsuite::proxqp::Settings<double> m_settings;
-  std::shared_ptr<proxsuite::proxqp::sparse::QP<double, int>> m_qp_ptr;
+  proxsuite::proxqp::Settings<double> settings_{};
+  std::shared_ptr<proxsuite::proxqp::sparse::QP<double, int>> qp_ptr_{nullptr};
 
   void initializeProblemImpl(
     const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
     const std::vector<double> & l, const std::vector<double> & u) override;
 
   std::vector<double> optimizeImpl() override;
 };
-}  // namespace qp
+}  // namespace autoware::common
 
 #endif  // QP_INTERFACE__PROXQP_INTERFACE_HPP_

common/qp_interface/include/qp_interface/qp_interface.hpp

@@ -18,28 +18,30 @@
 #include <Eigen/Core>
 
 #include <optional>
+#include <string>
 #include <vector>
 
-namespace qp
+namespace autoware::common
 {
 class QPInterface
 {
 public:
-  explicit QPInterface(const bool enable_warm_start) : m_enable_warm_start(enable_warm_start) {}
+  explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {}
 
   std::vector<double> optimize(
     const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
     const std::vector<double> & l, const std::vector<double> & u);
 
-  virtual int getIteration() const = 0;
-  virtual int getStatus() const = 0;
+  virtual bool isSolved() const = 0;
+  virtual int getIterationNumber() const = 0;
+  virtual std::string getStatus() const = 0;
 
   virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0;
   virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0;
   virtual void updateVerbose([[maybe_unused]] const bool verbose) {}
 
 protected:
-  bool m_enable_warm_start;
+  bool enable_warm_start_{false};
 
   void initializeProblem(
     const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
@@ -51,9 +53,9 @@ class QPInterface
 
   virtual std::vector<double> optimizeImpl() = 0;
 
-  std::optional<size_t> m_variables_num;
-  std::optional<size_t> m_constraints_num;
+  std::optional<size_t> variables_num_{std::nullopt};
+  std::optional<size_t> constraints_num_{std::nullopt};
 };
-}  // namespace qp
+}  // namespace autoware::common
 
 #endif  // QP_INTERFACE__QP_INTERFACE_HPP_

common/qp_interface/src/osqp_csc_matrix_conv.cpp

@@ -21,7 +21,7 @@
 #include <iostream>
 #include <vector>
 
-namespace qp
+namespace autoware::common
 {
 CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat)
 {
@@ -114,21 +114,21 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat)
 void printCSCMatrix(const CSC_Matrix & csc_mat)
 {
   std::cout << "[";
-  for (const c_float & val : csc_mat.m_vals) {
+  for (const c_float & val : csc_mat.vals_) {
     std::cout << val << ", ";
   }
   std::cout << "]\n";
 
   std::cout << "[";
-  for (const c_int & row : csc_mat.m_row_idxs) {
+  for (const c_int & row : csc_mat.row_idxs_) {
     std::cout << row << ", ";
   }
   std::cout << "]\n";
 
   std::cout << "[";
-  for (const c_int & col : csc_mat.m_col_idxs) {
+  for (const c_int & col : csc_mat.col_idxs_) {
     std::cout << col << ", ";
   }
   std::cout << "]\n";
 }
-}  // namespace qp
+}  // namespace autoware::common