refactor: update MPC CGMRES class to use shared_ptr for external refe…

…rence Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · May 21, 2024 · e02dfe1 · e02dfe1
1 parent 2aaa0b7
commit e02dfe1
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 6 deletions.
diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_cgmres.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_cgmres.hpp
@@ -158,13 +158,14 @@ class OCP_lateral_control
   ///
   void synchronize()
   {
-    // std::cerr << "\n\n\n synchronize is called \n\n\n" << std::endl;
     if (external_reference != nullptr) {
       u_ref[0] = std::atan(external_reference->curvature_in_reference_trajectory * wheel_base);
       curvature_in_reference_trajectory = external_reference->curvature_in_reference_trajectory;
-      std::cerr << "u_ref is updated as: " << u_ref[0] << std::endl;
-      std::cerr << "curvature_in_reference_trajectory is updated as: "
-                << curvature_in_reference_trajectory << std::endl;
+      // std::cerr << "u_ref is updated as: " << u_ref[0] << std::endl;
+      // std::cerr << "curvature_in_reference_trajectory is updated as: "
+      //           << curvature_in_reference_trajectory << std::endl;
+    } else {
+      // std::cerr << "external_reference is nullptr" << std::endl;
     }
   }
 

diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_cgmres.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_cgmres.hpp
@@ -25,6 +25,7 @@
 #include "osqp_interface/osqp_interface.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include <memory>
 #include <string>
 
 namespace autoware::motion::control::mpc_lateral_controller
@@ -80,6 +81,7 @@ class QPSolverCGMRES : public QPSolverInterface
   rclcpp::Logger logger_;
   cgmres::Logger cgmres_logger_;
   cgmres::SolverSettings settings_;
+  std::shared_ptr<cgmres::OCP_lateral_control::ExternalReference> external_reference_;
   cgmres::SingleShootingCGMRESSolver<cgmres::OCP_lateral_control, N, kmax> mpc_;
   cgmres::ZeroHorizonOCPSolver<cgmres::OCP_lateral_control, kmax_init> initializer_;
   bool is_initialized_ = false;

diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_cgmres.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_cgmres.cpp
@@ -33,9 +33,12 @@ QPSolverCGMRES::QPSolverCGMRES(const rclcpp::Logger & logger, const std::string
     1e-03,  // min_dummy
     0       // verbose_level
   },
-  mpc_(ocp_, cgmres::Horizon(0.1, 0.0), settings_),
+  external_reference_(std::make_shared<cgmres::OCP_lateral_control::ExternalReference>()),
   initializer_(ocp_, settings_)
 {
+  ocp_.external_reference = external_reference_;
+  mpc_ = cgmres::SingleShootingCGMRESSolver<cgmres::OCP_lateral_control, N, kmax>(
+    ocp_, cgmres::Horizon(0.1, 0.0), settings_);
 }
 
 bool QPSolverCGMRES::solveCGMRES(
@@ -62,6 +65,7 @@ bool QPSolverCGMRES::solveCGMRES(
   // set the external reference ptr
   ocp_.curvature_in_reference_trajectory = average_curvature;
   ocp_.u_ref[0] = std::atan(average_curvature * ocp_.wheel_base);
+  external_reference_->curvature_in_reference_trajectory = average_curvature;
   std::cerr << "average_curvature: " << average_curvature << std::endl;
   std::cerr << "ocp_.u_ref[0]: " << ocp_.u_ref[0] << std::endl;
   // ocp_.disp(std::cerr);

diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp
@@ -355,7 +355,7 @@ TEST_F(FakeNodeFixture, right_turn_convergence)
     tester.traj_pub->publish(ref_trajectory);
   };
 
-  const double curvature_sign = -0.05;
+  const double curvature_sign = -0.06;
   constexpr size_t iter_num = 10;
   for (size_t i = 0; i < iter_num; i++) {
     // curvature_sign = curvature_sign - 0.01;