conditional timer - MRM trigger

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

Author: PanConChicharron

common/osqp_interface/design/osqp_interface-design.md

@@ -54,7 +54,7 @@ The interface can be used in several ways:
 
    ```cpp
    std::tuple<std::vector<double>, std::vector<double>> result = osqp_interface.optimize();
-   std::vector<double> param = std::get<0>(result);
+   std::vector<double> param = result.primal_solution;
    double x_0 = param[0];
    double x_1 = param[1];
    ```

common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp

@@ -37,6 +37,44 @@ 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)
+  {
+    os << "CSC_Matrix: {\n";
+    os << "\tm_vals: [";
+
+    // Iterator-based loop for m_vals
+    for (auto it = std::begin(matrix.m_vals); it != std::end(matrix.m_vals); ++it) {
+      os << *it;  // Print the current element (dereference iterator)
+      if (std::next(it) != std::end(matrix.m_vals)) {  // Check if not the last element
+        os << ", ";
+      }
+    }
+    os << "],\n";
+
+    os << "\tm_row_idxs: [";
+    // Iterator-based loop for m_row_idxs
+    for (auto it = std::begin(matrix.m_row_idxs); it != std::end(matrix.m_row_idxs); ++it) {
+      os << *it;
+      if (std::next(it) != std::end(matrix.m_row_idxs)) {
+        os << ", ";
+      }
+    }
+    os << "],\n";
+
+    os << "\tm_col_idxs: [";
+    // Iterator-based loop for m_col_idxs
+    for (auto it = std::begin(matrix.m_col_idxs); it != std::end(matrix.m_col_idxs); ++it) {
+      os << *it;
+      if (std::next(it) != std::end(matrix.m_col_idxs)) {
+        os << ", ";
+      }
+    }
+    os << "]\n";
+
+    os << "}\n";
+    return os;
+  }
 };
 
 /// \brief Calculate CSC matrix from Eigen matrix

common/osqp_interface/include/osqp_interface/osqp_interface.hpp

@@ -36,6 +36,16 @@ namespace osqp
 {
 constexpr c_float INF = 1e30;
 
+struct OSQPResult
+{
+  std::vector<double> primal_solution;
+  std::vector<double> lagrange_multipliers;
+  int polish_status;
+  int solution_status;
+  int iteration_status;
+  int exit_flag;
+};
+
 /**
  * Implementation of a native C++ interface for the OSQP solver.
  *
@@ -56,7 +66,7 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
   int64_t m_exitflag;
 
   // Runs the solver on the stored problem.
-  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> solve();
+  OSQPResult solve();
 
   static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;
 
@@ -97,10 +107,10 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
   /// \details        std::tuple<std::vector<double>, std::vector<double>> result;
   /// \details        result = osqp_interface.optimize();
   /// \details   4. Access the optimized parameters.
-  /// \details        std::vector<float> param = std::get<0>(result);
+  /// \details        std::vector<float> param = result.primal_solution;
   /// \details        double x_0 = param[0];
   /// \details        double x_1 = param[1];
-  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> optimize();
+  OSQPResult optimize();
 
   /// \brief Solves convex quadratic programs (QPs) using the OSQP solver.
   /// \return The function returns a tuple containing the solution as two float vectors.
@@ -115,10 +125,10 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
   /// \details        std::tuple<std::vector<double>, std::vector<double>> result;
   /// \details        result = osqp_interface.optimize(P, A, q, l, u, 1e-6);
   /// \details   4. Access the optimized parameters.
-  /// \details        std::vector<float> param = std::get<0>(result);
+  /// \details        std::vector<float> param = result.primal_solution;
   /// \details        double x_0 = param[0];
   /// \details        double x_1 = param[1];
-  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> optimize(
+  OSQPResult 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/src/osqp_interface.cpp

@@ -363,11 +363,10 @@ int64_t OSQPInterface::initializeProblem(
   return m_exitflag;
 }
 
-std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
-OSQPInterface::solve()
+OSQPResult OSQPInterface::solve()
 {
   // Solve Problem
-  osqp_solve(m_work.get());
+  int32_t exit_flag = osqp_solve(m_work.get());
 
   /********************
    * EXTRACT SOLUTION
@@ -382,33 +381,36 @@ OSQPInterface::solve()
   int64_t status_iteration = m_work->info->iter;
 
   // Result tuple
-  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result =
-    std::make_tuple(
-      sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration);
+  OSQPResult result;
+
+  result.primal_solution = sol_primal;
+  result.lagrange_multipliers = sol_lagrange_multiplier;
+  result.polish_status = status_polish;
+  result.solution_status = status_solution;
+  result.iteration_status = status_iteration;
+  result.exit_flag = exit_flag;
 
   m_latest_work_info = *(m_work->info);
 
   return result;
 }
 
-std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
-OSQPInterface::optimize()
+OSQPResult OSQPInterface::optimize()
 {
   // Run the solver on the stored problem representation.
-  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();
+  OSQPResult result = solve();
   return result;
 }
 
-std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
-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)
 {
   // Allocate memory for problem
   initializeProblem(P, A, q, l, u);
 
   // Run the solver on the stored problem representation.
-  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();
+  OSQPResult result = solve();
 
   m_work.reset();
   m_work_initialized = false;

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 std::tuple<std::vector<double>, std::vector<double>, int, int, int> & result) {
-      EXPECT_EQ(std::get<2>(result), 1);  // polish succeeded
-      EXPECT_EQ(std::get<3>(result), 1);  // solution succeeded
-
-      static const auto ep = 1.0e-8;
-
-      const auto prime_val = std::get<0>(result);
-      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 = std::get<1>(result);
-      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();
@@ -73,20 +72,19 @@ TEST(TestOsqpInterface, BasicQp)
   {
     // Define problem during optimization
     autoware::common::osqp::OSQPInterface osqp;
-    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result =
-      osqp.optimize(P, A, q, l, u);
+    autoware::common::osqp::OSQPResult result = osqp.optimize(P, A, q, l, u);
     check_result(result);
   }
 
   {
     // Define problem during initialization
     autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6);
-    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result = osqp.optimize();
+    autoware::common::osqp::OSQPResult result = osqp.optimize();
     check_result(result);
   }
 
   {
-    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
+    autoware::common::osqp::OSQPResult result;
     // Dummy initial problem
     Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2);
     Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2);
@@ -107,12 +105,12 @@ TEST(TestOsqpInterface, BasicQp)
     CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
     CSC_Matrix A_csc = calCSCMatrix(A);
     autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6);
-    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result = osqp.optimize();
+    autoware::common::osqp::OSQPResult result = osqp.optimize();
     check_result(result);
   }
 
   {
-    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
+    autoware::common::osqp::OSQPResult result;
     // Dummy initial problem with csc matrix
     CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2));
     CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2));
@@ -132,7 +130,7 @@ TEST(TestOsqpInterface, BasicQp)
 
   // add warm startup
   {
-    std::tuple<std::vector<double>, std::vector<double>, int, int, int> result;
+    autoware::common::osqp::OSQPResult result;
     // Dummy initial problem with csc matrix
     CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2));
     CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2));
@@ -150,8 +148,8 @@ TEST(TestOsqpInterface, BasicQp)
     check_result(result);
 
     osqp.updateCheckTermination(1);
-    const auto primal_val = std::get<0>(result);
-    const auto dual_val = std::get<1>(result);
+    const auto primal_val = result.primal_solution;
+    const auto dual_val = result.lagrange_multipliers;
     for (size_t i = 0; i < primal_val.size(); ++i) {
       std::cerr << primal_val.at(i) << std::endl;
     }

control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp

@@ -54,11 +54,11 @@ bool QPSolverOSQP::solve(
   /* execute optimization */
   auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound);
 
-  std::vector<double> U_osqp = std::get<0>(result);
+  std::vector<double> U_osqp = result.primal_solution;
   u = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
     &U_osqp[0], static_cast<Eigen::Index>(U_osqp.size()), 1);
 
-  const int status_val = std::get<3>(result);
+  const int status_val = result.solution_status;
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
     return false;
@@ -71,7 +71,7 @@ bool QPSolverOSQP::solve(
   }
 
   // polish status: successful (1), unperformed (0), (-1) unsuccessful
-  int status_polish = std::get<2>(result);
+  int status_polish = result.polish_status;
   if (status_polish == -1 || status_polish == 0) {
     const auto s = (status_polish == 0) ? "Polish process is not performed in osqp."
                                         : "Polish process failed in osqp.";

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -265,6 +265,7 @@ def launch_setup(context, *args, **kwargs):
         composable_node_descriptions=[
             obstacle_velocity_limiter_component,
         ],
+        prefix="konsole -e gdb -ex run --args",
     )
 
     obstacle_stop_planner_loader = LoadComposableNodes(

planning/behavior_velocity_planner/package.xml

@@ -40,6 +40,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>grid_map_core</depend>
   <depend>jpn_signal_v2i_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>libboost-dev</depend>

planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

@@ -293,8 +293,8 @@ bool JerkFilteredSmoother::apply(
 
   // execute optimization
   const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
-  const std::vector<double> optval = std::get<0>(result);
-  const int status_val = std::get<3>(result);
+  const std::vector<double> optval = result.primal_solution;
+  const int status_val = result.solution_status;
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
     return false;
@@ -324,7 +324,7 @@ bool JerkFilteredSmoother::apply(
 
   qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]");
 
-  const int status_polish = std::get<2>(result);
+  const int status_polish = result.polish_status;
   if (status_polish != 1) {
     const auto msg = status_polish == 0    ? "unperformed"
                      : status_polish == -1 ? "unsuccessful"

planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp

@@ -189,8 +189,8 @@ bool L2PseudoJerkSmoother::apply(
 
   // [b0, b1, ..., bN, |  a0, a1, ..., aN, |
   //  delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN]
-  const std::vector<double> optval = std::get<0>(result);
-  const int status_val = std::get<3>(result);
+  const std::vector<double> optval = result.primal_solution;
+  const int status_val = result.solution_status;
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
     return false;

planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp

@@ -204,8 +204,8 @@ bool LinfPseudoJerkSmoother::apply(
 
   // [b0, b1, ..., bN, |  a0, a1, ..., aN, |
   //  delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN]
-  const std::vector<double> optval = std::get<0>(result);
-  const int status_val = std::get<3>(result);
+  const std::vector<double> optval = result.primal_solution;
+  const int status_val = result.solution_status;
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
     return false;