style(pre-commit): autofix

Author: pre-commit-ci[bot]

common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp

@@ -37,8 +37,9 @@ struct OSQP_INTERFACE_PUBLIC CSC_Matrix
   std::vector<c_int> m_row_idxs;
   /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer')
   std::vector<c_int> m_col_idxs;
-  
-  friend std::ostream& operator<<(std::ostream& os, const CSC_Matrix& matrix) {
+
+  friend std::ostream & operator<<(std::ostream & os, const CSC_Matrix & matrix)
+  {
     os << "CSC_Matrix: {\n";
     os << "\tm_vals: [";
     for (size_t i = 0; i < matrix.m_vals.size(); ++i) {

common/osqp_interface/include/osqp_interface/osqp_interface.hpp

@@ -36,7 +36,8 @@ namespace osqp
 {
 constexpr c_float INF = 1e30;
 
-struct OSQPResult {
+struct OSQPResult
+{
   std::vector<double> primal_solution;
   std::vector<double> lagrange_multipliers;
   int polish_status;

common/osqp_interface/src/osqp_interface.cpp

@@ -363,8 +363,7 @@ int64_t OSQPInterface::initializeProblem(
   return m_exitflag;
 }
 
-OSQPResult
-OSQPInterface::solve()
+OSQPResult OSQPInterface::solve()
 {
   // Solve Problem
   int32_t exit_flag = osqp_solve(m_work.get());
@@ -396,16 +395,14 @@ OSQPInterface::solve()
   return result;
 }
 
-OSQPResult
-OSQPInterface::optimize()
+OSQPResult OSQPInterface::optimize()
 {
   // Run the solver on the stored problem representation.
   OSQPResult result = solve();
   return result;
 }
 
-OSQPResult
-OSQPInterface::optimize(
+OSQPResult OSQPInterface::optimize(
   const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
   const std::vector<double> & l, const std::vector<double> & u)
 {

common/osqp_interface/test/test_osqp_interface.cpp

@@ -44,25 +44,24 @@ TEST(TestOsqpInterface, BasicQp)
   using autoware::common::osqp::calCSCMatrixTrapezoidal;
   using autoware::common::osqp::CSC_Matrix;
 
-  auto check_result =
-    [](const autoware::common::osqp::OSQPResult & result) {
-      EXPECT_EQ(result.polish_status, 1);  // polish succeeded
-      EXPECT_EQ(result.solution_status, 1);  // solution succeeded
-
-      static const auto ep = 1.0e-8;
-
-      const auto prime_val = result.primal_solution;
-      ASSERT_EQ(prime_val.size(), size_t(2));
-      EXPECT_NEAR(prime_val[0], 0.3, ep);
-      EXPECT_NEAR(prime_val[1], 0.7, ep);
-
-      const auto dual_val = result.lagrange_multipliers;
-      ASSERT_EQ(dual_val.size(), size_t(4));
-      EXPECT_NEAR(dual_val[0], -2.9, ep);
-      EXPECT_NEAR(dual_val[1], 0.0, ep);
-      EXPECT_NEAR(dual_val[2], 0.2, ep);
-      EXPECT_NEAR(dual_val[3], 0.0, ep);
-    };
+  auto check_result = [](const autoware::common::osqp::OSQPResult & result) {
+    EXPECT_EQ(result.polish_status, 1);    // polish succeeded
+    EXPECT_EQ(result.solution_status, 1);  // solution succeeded
+
+    static const auto ep = 1.0e-8;
+
+    const auto prime_val = result.primal_solution;
+    ASSERT_EQ(prime_val.size(), size_t(2));
+    EXPECT_NEAR(prime_val[0], 0.3, ep);
+    EXPECT_NEAR(prime_val[1], 0.7, ep);
+
+    const auto dual_val = result.lagrange_multipliers;
+    ASSERT_EQ(dual_val.size(), size_t(4));
+    EXPECT_NEAR(dual_val[0], -2.9, ep);
+    EXPECT_NEAR(dual_val[1], 0.0, ep);
+    EXPECT_NEAR(dual_val[2], 0.2, ep);
+    EXPECT_NEAR(dual_val[3], 0.0, ep);
+  };
 
   const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished();
   const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished();

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -57,7 +57,8 @@ struct Bounds
     upper_bound -= offset;
   }
 
-  friend std::ostream& operator<<(std::ostream& os, const Bounds& bounds) {
+  friend std::ostream & operator<<(std::ostream & os, const Bounds & bounds)
+  {
     os << "Bounds: {\n";
     os << "\tlower_bound: " << bounds.lower_bound << ",\n";
     os << "\tupper_bound: " << bounds.upper_bound << "\n";
@@ -73,7 +74,8 @@ struct KinematicState
 
   Eigen::Vector2d toEigenVector() const { return Eigen::Vector2d{lat, yaw}; }
 
-  friend std::ostream& operator<<(std::ostream& os, const KinematicState& state) {
+  friend std::ostream & operator<<(std::ostream & os, const KinematicState & state)
+  {
     os << "KinematicState: {\n";
     os << "\tlat: " << state.lat << ",\n";
     os << "\tyaw: " << state.yaw << "\n";
@@ -107,16 +109,17 @@ struct ReferencePoint
 
   double getYaw() const { return tf2::getYaw(pose.orientation); }
 
-  friend std::ostream& operator<<(std::ostream& os, const ReferencePoint& ref_point) {
+  friend std::ostream & operator<<(std::ostream & os, const ReferencePoint & ref_point)
+  {
     os << "ReferencePoint: {\n";
     os << "\tpose: " << ref_point.pose.position.x << ", " << ref_point.pose.position.y << ",\n";
     os << "\tlongitudinal_velocity_mps: " << ref_point.longitudinal_velocity_mps << ",\n";
     os << "\tcurvature: " << ref_point.curvature << ",\n";
     os << "\tdelta_arc_length: " << ref_point.delta_arc_length << ",\n";
     os << "\talpha: " << ref_point.alpha << ",\n";
-    os << "\tbounds: " << ref_point.bounds << ",\n"; // Assuming Bounds is printable
+    os << "\tbounds: " << ref_point.bounds << ",\n";  // Assuming Bounds is printable
     os << "\tbeta: [";
-    for (const auto& b : ref_point.beta) {
+    for (const auto & b : ref_point.beta) {
       if (b) {
         os << *b << ", ";
       } else {
@@ -126,28 +129,29 @@ struct ReferencePoint
     os << "],\n";
     os << "\tnormalized_avoidance_cost: " << ref_point.normalized_avoidance_cost << ",\n";
     os << "\tbounds_on_constraints: [";
-    for (const auto& b : ref_point.bounds_on_constraints) {
-      os << b << ", "; // Assuming Bounds is printable
+    for (const auto & b : ref_point.bounds_on_constraints) {
+      os << b << ", ";  // Assuming Bounds is printable
     }
     os << "],\n";
     os << "\tpose_on_constraints: [";
-    for (const auto& p : ref_point.pose_on_constraints) {
+    for (const auto & p : ref_point.pose_on_constraints) {
       os << "(" << p.position.x << ", " << p.position.y << ") , ";
     }
     os << "],\n";
     os << "\tfixed_kinematic_state: ";
     if (ref_point.fixed_kinematic_state) {
-      os << *ref_point.fixed_kinematic_state; // Assuming KinematicState is printable
+      os << *ref_point.fixed_kinematic_state;  // Assuming KinematicState is printable
     } else {
       os << "nullopt";
     }
     os << ",\n";
-    os << "\toptimized_kinematic_state: " << ref_point.optimized_kinematic_state << ",\n"; // Assuming KinematicState is printable
+    os << "\toptimized_kinematic_state: " << ref_point.optimized_kinematic_state
+       << ",\n";  // Assuming KinematicState is printable
     os << "\toptimized_input: " << ref_point.optimized_input << ",\n";
     os << "\tslack_variables: ";
     if (ref_point.slack_variables) {
       os << "[";
-      for (const auto& s : *ref_point.slack_variables) {
+      for (const auto & s : *ref_point.slack_variables) {
         os << s << ", ";
       }
       os << "]";
@@ -194,8 +198,9 @@ class MPTOptimizer
   {
     Eigen::SparseMatrix<double> Q;
     Eigen::SparseMatrix<double> R;
-    
-    friend std::ostream& operator<<(std::ostream& os, const ValueMatrix& matrix) {
+
+    friend std::ostream & operator<<(std::ostream & os, const ValueMatrix & matrix)
+    {
       os << "ValueMatrix: {\n";
       os << "\tQ: (Sparse Matrix):" << matrix.Q;
       os << "\tR: (Sparse Matrix):" << matrix.R;
@@ -208,23 +213,25 @@ class MPTOptimizer
   {
     Eigen::MatrixXd hessian;
     Eigen::VectorXd gradient;
-  
-    friend std::ostream& operator<<(std::ostream& os, const ObjectiveMatrix& matrix) {
+
+    friend std::ostream & operator<<(std::ostream & os, const ObjectiveMatrix & matrix)
+    {
       os << "ObjectiveMatrix: {\n";
       os << "\thessian:\n" << matrix.hessian << "\n";
       os << "\tgradient:\n" << matrix.gradient << "\n";
       os << "}\n";
       return os;
     }
   };
-  
+
   struct ConstraintMatrix
   {
     Eigen::MatrixXd linear;
     Eigen::VectorXd lower_bound;
     Eigen::VectorXd upper_bound;
-  
-    friend std::ostream& operator<<(std::ostream& os, const ConstraintMatrix& matrix) {
+
+    friend std::ostream & operator<<(std::ostream & os, const ConstraintMatrix & matrix)
+    {
       os << "ConstraintMatrix: {\n";
       os << "\tlinear:\n" << matrix.linear << "\n";
       os << "\tlower_bound:\n" << matrix.lower_bound << "\n";

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp

@@ -35,7 +35,8 @@ class StateEquationGenerator
     Eigen::MatrixXd B;
     Eigen::VectorXd W;
 
-    friend std::ostream& operator<<(std::ostream& os, const Matrix& matrix) {
+    friend std::ostream & operator<<(std::ostream & os, const Matrix & matrix)
+    {
       os << "Matrix: {\n";
       os << "\tA:\n" << matrix.A << "\n";
       os << "\tB:\n" << matrix.B << "\n";

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/conditional_timer.hpp

@@ -3,27 +3,32 @@
 #include <chrono>
 #include <optional>
 
-class ConditionalTimer {
+class ConditionalTimer
+{
 public:
-    void update(bool condition) {
-        if (condition && !start_time_.has_value()) {
-            // Condition met, start the timer
-            start_time_ = std::chrono::high_resolution_clock::now();
-        } else if (!condition && start_time_.has_value()) {
-            // Condition no longer met, stop the timer
-            start_time_ = std::nullopt;
-        }
+  void update(bool condition)
+  {
+    if (condition && !start_time_.has_value()) {
+      // Condition met, start the timer
+      start_time_ = std::chrono::high_resolution_clock::now();
+    } else if (!condition && start_time_.has_value()) {
+      // Condition no longer met, stop the timer
+      start_time_ = std::nullopt;
     }
+  }
 
-    std::chrono::duration<double> getElapsedTime() const {
-        if (start_time_.has_value()) {
-            auto current_time = std::chrono::high_resolution_clock::now();
-            return current_time - *start_time_;
-        } else {
-            return std::chrono::duration<double>(0.0);;
-        }
+  std::chrono::duration<double> getElapsedTime() const
+  {
+    if (start_time_.has_value()) {
+      auto current_time = std::chrono::high_resolution_clock::now();
+      return current_time - *start_time_;
+    } else {
+      return std::chrono::duration<double>(0.0);
+      ;
     }
+  }
 
 private:
-    std::optional<std::chrono::time_point<std::chrono::high_resolution_clock>> start_time_{std::nullopt};
-};
+  std::optional<std::chrono::time_point<std::chrono::high_resolution_clock>> start_time_{
+    std::nullopt};
+};

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -20,6 +20,7 @@
 #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp"
 #include "tf2/utils.h"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
 #include <rclcpp/logging.hpp>
 
 #include <algorithm>
@@ -116,13 +117,14 @@ std::vector<double> toStdVector(const Eigen::VectorXd & eigen_vec)
   return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()};
 }
 
-std::string vectorToString(const std::vector<double>& vec) {
+std::string vectorToString(const std::vector<double> & vec)
+{
   std::ostringstream oss;
   if (!vec.empty()) {
-      oss << vec.front();  // Add the first element
-      for (size_t i = 1; i < vec.size(); ++i) {
-          oss << "," << vec[i];  // Add the rest with commas
-      }
+    oss << vec.front();  // Add the first element
+    for (size_t i = 1; i < vec.size(); ++i) {
+      oss << "," << vec[i];  // Add the rest with commas
+    }
   }
   return oss.str();
 }
@@ -513,21 +515,19 @@ std::pair<std::vector<TrajectoryPoint>, int> MPTOptimizer::optimizeTrajectory(
   if (!optimized_variables) {
     RCLCPP_INFO_EXPRESSION(
       logger_, enable_debug_info_, "return std::nullopt since could not solve qp");
-    
-    RCLCPP_INFO(
-      logger_, "Inputs causing failure:"
-    );
+
+    RCLCPP_INFO(logger_, "Inputs causing failure:");
     RCLCPP_INFO_STREAM(logger_, "ref_points: ");
-    
-    for (const auto& ref_point : ref_points) {
+
+    for (const auto & ref_point : ref_points) {
       std::cout << "\t" << ref_point << "\n";
     }
 
     RCLCPP_INFO_STREAM(logger_, "mpt_mat: " << mpt_mat);
     RCLCPP_INFO_STREAM(logger_, "val_mat: " << val_mat);
     RCLCPP_INFO_STREAM(logger_, "obj_mat: " << obj_mat);
     RCLCPP_INFO_STREAM(logger_, "const_mat: " << const_mat);
-    
+
     return std::make_pair<std::vector<TrajectoryPoint>, int>(get_prev_optimized_traj_points(), -1);
   }
 
@@ -1535,10 +1535,8 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
     autoware::common::osqp::calCSCMatrixTrapezoidal(H);
   const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A);
   if (
-    prev_solution_status_ == 1 && 
-    mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() &&
-    prev_mat_m_ == A.rows()
-  ) {
+    prev_solution_status_ == 1 && mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() &&
+    prev_mat_m_ == A.rows()) {
     RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "warm start");
     osqp_solver_ptr_->updateCscP(P_csc);
     osqp_solver_ptr_->updateQ(f);
@@ -1563,24 +1561,23 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
   const int solution_status = osqp_result.solution_status;
   prev_solution_status_ = solution_status;
   if (solution_status != 1) {
-
     const std::vector<double> lagrange_multiplier = osqp_result.lagrange_multipliers;
     const int status_polish = osqp_result.polish_status;
     const int exit_flag = osqp_result.exit_flag;
-    
+
     RCLCPP_INFO(logger_, "optimization failed: result contains NaN values");
     RCLCPP_INFO(logger_, "Problem details:");
     RCLCPP_INFO_STREAM(logger_, "P_csc: " << P_csc);
     RCLCPP_INFO_STREAM(logger_, "f: " << vectorToString(f));
     RCLCPP_INFO_STREAM(logger_, "A_csc: " << A_csc);
-    RCLCPP_INFO_STREAM(logger_, "lower_bound: " << vectorToString(lower_bound)); 
+    RCLCPP_INFO_STREAM(logger_, "lower_bound: " << vectorToString(lower_bound));
     RCLCPP_INFO_STREAM(logger_, "upper_bound: " << vectorToString(upper_bound));
     RCLCPP_INFO(logger_, "Solver details:");
     RCLCPP_INFO_STREAM(logger_, "lagrange_multiplier: " << vectorToString(lagrange_multiplier));
     RCLCPP_INFO_STREAM(logger_, "status_polish: " << status_polish);
     RCLCPP_INFO_STREAM(logger_, "solution_status: " << solution_status);
     RCLCPP_INFO_STREAM(logger_, "exit_flag: " << exit_flag);
-    
+
     osqp_solver_ptr_->logUnsolvedStatus("[MPT]");
     return std::nullopt;
   }
@@ -1595,20 +1592,19 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
 
   const auto has_nan = std::any_of(
     optimization_result.begin(), optimization_result.end(),
-    [](const auto v) { return std::isnan(v); }
-  );
+    [](const auto v) { return std::isnan(v); });
 
   if (has_nan) {
     const std::vector<double> lagrange_multiplier = osqp_result.lagrange_multipliers;
     const int status_polish = osqp_result.polish_status;
     const int exit_flag = osqp_result.exit_flag;
-    
+
     RCLCPP_INFO(logger_, "optimization failed: result contains NaN values");
     RCLCPP_INFO(logger_, "Problem details:");
     RCLCPP_INFO_STREAM(logger_, "P_csc: " << P_csc);
     RCLCPP_INFO_STREAM(logger_, "f: " << vectorToString(f));
     RCLCPP_INFO_STREAM(logger_, "A_csc: " << A_csc);
-    RCLCPP_INFO_STREAM(logger_, "lower_bound: " << vectorToString(lower_bound)); 
+    RCLCPP_INFO_STREAM(logger_, "lower_bound: " << vectorToString(lower_bound));
     RCLCPP_INFO_STREAM(logger_, "upper_bound: " << vectorToString(upper_bound));
     RCLCPP_INFO(logger_, "Solver details:");
     RCLCPP_INFO_STREAM(logger_, "optimization_result: " << vectorToString(optimization_result));