mpc compile ok.

src/decition/mpccontroller/main.cpp

@@ -1,8 +1,14 @@
 #include <QCoreApplication>
 
+
+#include "mpctest.h"
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    MPCTest mpct;
+    mpct.InitializeAndCalculate();
+
     return a.exec();
 }

src/decition/mpccontroller/mpc.cpp

@@ -793,7 +793,7 @@ MPCMatrix MPC::generateMPCMatrix(
     // get reference input (feed-forward)
     m_vehicle_model_ptr->setCurvature(ref_smooth_k);
     m_vehicle_model_ptr->calculateReferenceInput(Uref);
-    if (std::fabs(Uref(0, 0)) < MPCUtils::deg2rad(m_param.zero_ff_steer_deg)) {
+    if (std::fabs(Uref(0, 0)) < (m_param.zero_ff_steer_deg * M_PI/180.0) ) {
       Uref(0, 0) = 0.0;  // ignore curvature noise
     }
     m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref;

src/decition/mpccontroller/mpc_lateral_controller.cpp

@@ -191,10 +191,13 @@ MpcLateralController::MpcLateralController()
   m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad;
 
   // steer rate limit depending on curvature
-  const auto steer_rate_lim_dps_list_by_curvature =
-      node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_curvature");
-  const auto curvature_list_for_steer_rate_lim =
-      node.declare_parameter<std::vector<double>>("curvature_list_for_steer_rate_lim");
+
+  std::vector<double> steer_rate_lim_dps_list_by_curvature;
+  std::vector<double> curvature_list_for_steer_rate_lim;
+//  const auto steer_rate_lim_dps_list_by_curvature =
+//      node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_curvature");
+//  const auto curvature_list_for_steer_rate_lim =
+//      node.declare_parameter<std::vector<double>>("curvature_list_for_steer_rate_lim");
   for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) {
     m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back(
         curvature_list_for_steer_rate_lim.at(i),
@@ -202,23 +205,28 @@ MpcLateralController::MpcLateralController()
   }
 
   // steer rate limit depending on velocity
-  const auto steer_rate_lim_dps_list_by_velocity =
-      node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_velocity");
-  const auto velocity_list_for_steer_rate_lim =
-      node.declare_parameter<std::vector<double>>("velocity_list_for_steer_rate_lim");
+  std::vector<double> steer_rate_lim_dps_list_by_velocity;
+  std::vector<double> velocity_list_for_steer_rate_lim;
+//  const auto steer_rate_lim_dps_list_by_velocity =
+//      node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_velocity");
+//  const auto velocity_list_for_steer_rate_lim =
+//      node.declare_parameter<std::vector<double>>("velocity_list_for_steer_rate_lim");
   for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) {
     m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back(
         velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad);
   }
 
   /* vehicle model setup */
+//  auto vehicle_model_ptr =
+//      createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node);
   auto vehicle_model_ptr =
-      createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node);
+      createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, xp);
   m_mpc.setVehicleModel(vehicle_model_ptr);
 
   /* QP solver setup */
   m_mpc.setVehicleModel(vehicle_model_ptr);
-  auto qpsolver_ptr = createQPSolverInterface(node);
+//  auto qpsolver_ptr = createQPSolverInterface(node);
+  auto qpsolver_ptr = createQPSolverInterface(xp);
   m_mpc.setQPSolver(qpsolver_ptr);
 
   /* delay compensation */
@@ -232,7 +240,8 @@ MpcLateralController::MpcLateralController()
   /* steering offset compensation */
   enable_auto_steering_offset_removal_ =
       dp_bool("steering_offset.enable_auto_steering_offset_removal");
-  steering_offset_ = createSteerOffsetEstimator(wheelbase, node);
+//  steering_offset_ = createSteerOffsetEstimator(wheelbase, node);
+  steering_offset_ = createSteerOffsetEstimator(wheelbase, xp);
 
   /* initialize low-pass filter */
   {
@@ -242,42 +251,81 @@ MpcLateralController::MpcLateralController()
   }
 
   // ego nearest index search
+//  const auto check_and_get_param = [&](const auto & param) {
+//      return node.has_parameter(param) ? node.get_parameter(param).as_double() : dp_double(param);
+//  };
   const auto check_and_get_param = [&](const auto & param) {
-      return node.has_parameter(param) ? node.get_parameter(param).as_double() : dp_double(param);
+      return xp.GetParam(param,0.0);
   };
   m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold");
   m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold");
   m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold;
   m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold;
 
-  m_pub_predicted_traj = node.create_publisher<Trajectory>("~/output/predicted_trajectory", 1);
-  m_pub_debug_values =
-      node.create_publisher<Float32MultiArrayStamped>("~/output/lateral_diagnostic", 1);
-  m_pub_steer_offset = node.create_publisher<Float32Stamped>("~/output/estimated_steer_offset", 1);
+//  m_pub_predicted_traj = node.create_publisher<Trajectory>("~/output/predicted_trajectory", 1);
+//  m_pub_debug_values =
+//      node.create_publisher<Float32MultiArrayStamped>("~/output/lateral_diagnostic", 1);
+//  m_pub_steer_offset = node.create_publisher<Float32Stamped>("~/output/estimated_steer_offset", 1);
 
-  declareMPCparameters(node);
+//  declareMPCparameters(node);
+  declareMPCparameters(xp);
 
   /* get parameter updates */
-  using std::placeholders::_1;
-  m_set_param_res =
-      node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1));
+//  using std::placeholders::_1;
+//  m_set_param_res =
+//      node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1));
 
   m_mpc.initializeSteeringPredictor();
 
-  m_mpc.setLogger(logger_);
-  m_mpc.setClock(clock_);
+//  m_mpc.setLogger(logger_);
+//  m_mpc.setClock(clock_);
 }
 
 MpcLateralController::~MpcLateralController()
 {
 }
 
-std::shared_ptr<VehicleModelInterface> MpcLateralController::createVehicleModel(
-  const double wheelbase, const double steer_lim, const double steer_tau, rclcpp::Node & node)
+//std::shared_ptr<VehicleModelInterface> MpcLateralController::createVehicleModel(
+//  const double wheelbase, const double steer_lim, const double steer_tau, rclcpp::Node & node)
+//{
+//  std::shared_ptr<VehicleModelInterface> vehicle_model_ptr;
+
+//  const std::string vehicle_model_type = node.declare_parameter<std::string>("vehicle_model_type");
+
+//  if (vehicle_model_type == "kinematics") {
+//    vehicle_model_ptr = std::make_shared<KinematicsBicycleModel>(wheelbase, steer_lim, steer_tau);
+//    return vehicle_model_ptr;
+//  }
+
+//  if (vehicle_model_type == "kinematics_no_delay") {
+//    vehicle_model_ptr = std::make_shared<KinematicsBicycleModelNoDelay>(wheelbase, steer_lim);
+//    return vehicle_model_ptr;
+//  }
+
+//  if (vehicle_model_type == "dynamics") {
+//    const double mass_fl = node.declare_parameter<double>("vehicle.mass_fl");
+//    const double mass_fr = node.declare_parameter<double>("vehicle.mass_fr");
+//    const double mass_rl = node.declare_parameter<double>("vehicle.mass_rl");
+//    const double mass_rr = node.declare_parameter<double>("vehicle.mass_rr");
+//    const double cf = node.declare_parameter<double>("vehicle.cf");
+//    const double cr = node.declare_parameter<double>("vehicle.cr");
+
+//    // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time
+//    vehicle_model_ptr =
+//      std::make_shared<DynamicsBicycleModel>(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr);
+//    return vehicle_model_ptr;
+//  }
+
+//  RCLCPP_ERROR(logger_, "vehicle_model_type is undefined");
+//  return vehicle_model_ptr;
+//}
+
+std::shared_ptr<VehicleModelInterface>  MpcLateralController::createVehicleModel(
+    const double wheelbase, const double steer_lim, const double steer_tau, iv::xmlparam::Xmlparam & xp)
 {
   std::shared_ptr<VehicleModelInterface> vehicle_model_ptr;
 
-  const std::string vehicle_model_type = node.declare_parameter<std::string>("vehicle_model_type");
+  const std::string vehicle_model_type = xp.GetParam("vehicle_model_type","kinematics") ;//node.declare_parameter<std::string>("vehicle_model_type");
 
   if (vehicle_model_type == "kinematics") {
     vehicle_model_ptr = std::make_shared<KinematicsBicycleModel>(wheelbase, steer_lim, steer_tau);
@@ -290,29 +338,58 @@ std::shared_ptr<VehicleModelInterface> MpcLateralController::createVehicleModel(
   }
 
   if (vehicle_model_type == "dynamics") {
-    const double mass_fl = node.declare_parameter<double>("vehicle.mass_fl");
-    const double mass_fr = node.declare_parameter<double>("vehicle.mass_fr");
-    const double mass_rl = node.declare_parameter<double>("vehicle.mass_rl");
-    const double mass_rr = node.declare_parameter<double>("vehicle.mass_rr");
-    const double cf = node.declare_parameter<double>("vehicle.cf");
-    const double cr = node.declare_parameter<double>("vehicle.cr");
+//    const double mass_fl = node.declare_parameter<double>("vehicle.mass_fl");
+//    const double mass_fr = node.declare_parameter<double>("vehicle.mass_fr");
+//    const double mass_rl = node.declare_parameter<double>("vehicle.mass_rl");
+//    const double mass_rr = node.declare_parameter<double>("vehicle.mass_rr");
+//    const double cf = node.declare_parameter<double>("vehicle.cf");
+//    const double cr = node.declare_parameter<double>("vehicle.cr");
+
+    const double mass_fl = xp.GetParam("vehicle.mass_fl",0.0);
+    const double mass_fr = xp.GetParam("vehicle.mass_fr",0.0);
+    const double mass_rl = xp.GetParam("vehicle.mass_rl",0.0);
+    const double mass_rr = xp.GetParam("vehicle.mass_rr",0.0);
+    const double cf = xp.GetParam("vehicle.cf",0.0);
+    const double cr = xp.GetParam("vehicle.cr",0.0);
 
     // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time
     vehicle_model_ptr =
-      std::make_shared<DynamicsBicycleModel>(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr);
+        std::make_shared<DynamicsBicycleModel>(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr);
     return vehicle_model_ptr;
   }
 
-  RCLCPP_ERROR(logger_, "vehicle_model_type is undefined");
+  std::cout<<"vehicle_model_type is undefined"<<std::endl;
+ // RCLCPP_ERROR(logger_, "vehicle_model_type is undefined");
   return vehicle_model_ptr;
 }
 
+//std::shared_ptr<QPSolverInterface> MpcLateralController::createQPSolverInterface(
+//  rclcpp::Node & node)
+//{
+//  std::shared_ptr<QPSolverInterface> qpsolver_ptr;
+
+//  const std::string qp_solver_type = node.declare_parameter<std::string>("qp_solver_type");
+
+//  if (qp_solver_type == "unconstraint_fast") {
+//    qpsolver_ptr = std::make_shared<QPSolverEigenLeastSquareLLT>();
+//    return qpsolver_ptr;
+//  }
+
+//  if (qp_solver_type == "osqp") {
+//    qpsolver_ptr = std::make_shared<QPSolverOSQP>(logger_);
+//    return qpsolver_ptr;
+//  }
+
+//  RCLCPP_ERROR(logger_, "qp_solver_type is undefined");
+//  return qpsolver_ptr;
+//}
+
 std::shared_ptr<QPSolverInterface> MpcLateralController::createQPSolverInterface(
-  rclcpp::Node & node)
+    iv::xmlparam::Xmlparam & xp)
 {
   std::shared_ptr<QPSolverInterface> qpsolver_ptr;
 
-  const std::string qp_solver_type = node.declare_parameter<std::string>("qp_solver_type");
+  const std::string qp_solver_type = xp.GetParam("qp_solver_type","") ;//node.declare_parameter<std::string>("qp_solver_type");
 
   if (qp_solver_type == "unconstraint_fast") {
     qpsolver_ptr = std::make_shared<QPSolverEigenLeastSquareLLT>();
@@ -324,20 +401,34 @@ std::shared_ptr<QPSolverInterface> MpcLateralController::createQPSolverInterface
     return qpsolver_ptr;
   }
 
-  RCLCPP_ERROR(logger_, "qp_solver_type is undefined");
+  std::cout<<"qp_solver_type is undefined"<<std::endl;
+//  RCLCPP_ERROR(logger_, "qp_solver_type is undefined");
   return qpsolver_ptr;
 }
 
+//std::shared_ptr<SteeringOffsetEstimator> MpcLateralController::createSteerOffsetEstimator(
+//  const double wheelbase, rclcpp::Node & node)
+//{
+//  const std::string ns = "steering_offset.";
+//  const auto vel_thres = node.declare_parameter<double>(ns + "update_vel_threshold");
+//  const auto steer_thres = node.declare_parameter<double>(ns + "update_steer_threshold");
+//  const auto limit = node.declare_parameter<double>(ns + "steering_offset_limit");
+//  const auto num = node.declare_parameter<int>(ns + "average_num");
+//  steering_offset_ =
+//    std::make_shared<SteeringOffsetEstimator>(wheelbase, num, vel_thres, steer_thres, limit);
+//  return steering_offset_;
+//}
+
 std::shared_ptr<SteeringOffsetEstimator> MpcLateralController::createSteerOffsetEstimator(
-  const double wheelbase, rclcpp::Node & node)
+    const double wheelbase, iv::xmlparam::Xmlparam & xp)
 {
   const std::string ns = "steering_offset.";
-  const auto vel_thres = node.declare_parameter<double>(ns + "update_vel_threshold");
-  const auto steer_thres = node.declare_parameter<double>(ns + "update_steer_threshold");
-  const auto limit = node.declare_parameter<double>(ns + "steering_offset_limit");
-  const auto num = node.declare_parameter<int>(ns + "average_num");
+  const auto vel_thres = xp.GetParam(ns + "update_vel_threshold",0.0);
+  const auto steer_thres = xp.GetParam(ns + "update_steer_threshold",0.0);
+  const auto limit = xp.GetParam(ns + "steering_offset_limit",0.0);
+  const auto num = xp.GetParam(ns + "average_num",0);
   steering_offset_ =
-    std::make_shared<SteeringOffsetEstimator>(wheelbase, num, vel_thres, steer_thres, limit);
+      std::make_shared<SteeringOffsetEstimator>(wheelbase, num, vel_thres, steer_thres, limit);
   return steering_offset_;
 }
 
@@ -620,12 +711,50 @@ bool MpcLateralController::isMpcConverged()
   return (max_steering_value - min_steering_value) < m_mpc_converged_threshold_rps;
 }
 
-void MpcLateralController::declareMPCparameters(rclcpp::Node & node)
+//void MpcLateralController::declareMPCparameters(rclcpp::Node & node)
+//{
+//  m_mpc.m_param.prediction_horizon = node.declare_parameter<int>("mpc_prediction_horizon");
+//  m_mpc.m_param.prediction_dt = node.declare_parameter<double>("mpc_prediction_dt");
+
+//  const auto dp = [&](const auto & param) { return node.declare_parameter<double>(param); };
+
+//  auto & nw = m_mpc.m_param.nominal_weight;
+//  nw.lat_error = dp("mpc_weight_lat_error");
+//  nw.heading_error = dp("mpc_weight_heading_error");
+//  nw.heading_error_squared_vel = dp("mpc_weight_heading_error_squared_vel");
+//  nw.steering_input = dp("mpc_weight_steering_input");
+//  nw.steering_input_squared_vel = dp("mpc_weight_steering_input_squared_vel");
+//  nw.lat_jerk = dp("mpc_weight_lat_jerk");
+//  nw.steer_rate = dp("mpc_weight_steer_rate");
+//  nw.steer_acc = dp("mpc_weight_steer_acc");
+//  nw.terminal_lat_error = dp("mpc_weight_terminal_lat_error");
+//  nw.terminal_heading_error = dp("mpc_weight_terminal_heading_error");
+
+//  auto & lcw = m_mpc.m_param.low_curvature_weight;
+//  lcw.lat_error = dp("mpc_low_curvature_weight_lat_error");
+//  lcw.heading_error = dp("mpc_low_curvature_weight_heading_error");
+//  lcw.heading_error_squared_vel = dp("mpc_low_curvature_weight_heading_error_squared_vel");
+//  lcw.steering_input = dp("mpc_low_curvature_weight_steering_input");
+//  lcw.steering_input_squared_vel = dp("mpc_low_curvature_weight_steering_input_squared_vel");
+//  lcw.lat_jerk = dp("mpc_low_curvature_weight_lat_jerk");
+//  lcw.steer_rate = dp("mpc_low_curvature_weight_steer_rate");
+//  lcw.steer_acc = dp("mpc_low_curvature_weight_steer_acc");
+//  m_mpc.m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature");
+
+//  m_mpc.m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg");
+//  m_mpc.m_param.acceleration_limit = dp("mpc_acceleration_limit");
+//  m_mpc.m_param.velocity_time_constant = dp("mpc_velocity_time_constant");
+//  m_mpc.m_param.min_prediction_length = dp("mpc_min_prediction_length");
+//}
+
+void MpcLateralController::declareMPCparameters(iv::xmlparam::Xmlparam  & xp)
 {
-  m_mpc.m_param.prediction_horizon = node.declare_parameter<int>("mpc_prediction_horizon");
-  m_mpc.m_param.prediction_dt = node.declare_parameter<double>("mpc_prediction_dt");
+  m_mpc.m_param.prediction_horizon = xp.GetParam("mpc_prediction_horizon",1);// node.declare_parameter<int>("mpc_prediction_horizon");
+  m_mpc.m_param.prediction_dt = xp.GetParam("mpc_prediction_dt",1.0);// node.declare_parameter<double>("mpc_prediction_dt");
+
+ // const auto dp = [&](const auto & param) { return node.declare_parameter<double>(param); };
+  const auto dp = [&](const auto & param) { return xp.GetParam(param,1.0); };
 
-  const auto dp = [&](const auto & param) { return node.declare_parameter<double>(param); };
 
   auto & nw = m_mpc.m_param.nominal_weight;
   nw.lat_error = dp("mpc_weight_lat_error");

src/decition/mpccontroller/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -154,15 +154,19 @@ private:
    * @param node Reference to the node.
    * @return Pointer to the created vehicle model.
    */
+//  std::shared_ptr<VehicleModelInterface> createVehicleModel(
+//    const double wheelbase, const double steer_lim, const double steer_tau, rclcpp::Node & node);
+
   std::shared_ptr<VehicleModelInterface> createVehicleModel(
-    const double wheelbase, const double steer_lim, const double steer_tau, rclcpp::Node & node);
+      const double wheelbase, const double steer_lim, const double steer_tau, iv::xmlparam::Xmlparam & xp);
 
   /**
    * @brief Create the quadratic problem solver interface.
    * @param node Reference to the node.
    * @return Pointer to the created QP solver interface.
    */
-  std::shared_ptr<QPSolverInterface> createQPSolverInterface(rclcpp::Node & node);
+//  std::shared_ptr<QPSolverInterface> createQPSolverInterface(rclcpp::Node & node);
+  std::shared_ptr<QPSolverInterface> createQPSolverInterface(iv::xmlparam::Xmlparam & xp);
 
   /**
    * @brief Create the steering offset estimator for offset compensation.
@@ -170,8 +174,10 @@ private:
    * @param node Reference to the node.
    * @return Pointer to the created steering offset estimator.
    */
+//  std::shared_ptr<SteeringOffsetEstimator> createSteerOffsetEstimator(
+//    const double wheelbase, rclcpp::Node & node);
   std::shared_ptr<SteeringOffsetEstimator> createSteerOffsetEstimator(
-    const double wheelbase, rclcpp::Node & node);
+      const double wheelbase, iv::xmlparam::Xmlparam & xp);
 
   /**
    * @brief Check if all necessary data is received and ready to run the control.
@@ -263,7 +269,8 @@ private:
    * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly.
    * @param node Reference to the node.
    */
-  void declareMPCparameters(rclcpp::Node & node);
+//  void declareMPCparameters(rclcpp::Node & node);
+  void declareMPCparameters(iv::xmlparam::Xmlparam  & xp);
 
   /**
    * @brief Callback function called when parameters are changed outside of the node.

src/decition/mpccontroller/mpctest.cpp

@@ -0,0 +1,48 @@
+#include "mpctest.h"
+
+MPCTest::MPCTest()
+{
+    SetUp();
+}
+
+void MPCTest::InitializeAndCalculate()
+{
+    MPC mpc;
+
+    std::shared_ptr<VehicleModelInterface> vehicle_model_ptr =
+        std::make_shared<KinematicsBicycleModel>(wheelbase, steer_limit, steer_tau);
+    mpc.setVehicleModel(vehicle_model_ptr);
+
+    std::shared_ptr<QPSolverInterface> qpsolver_ptr = std::make_shared<QPSolverEigenLeastSquareLLT>();
+    mpc.setQPSolver(qpsolver_ptr);
+
+    std::cout<<" haveVehicleModel : "<<mpc.hasVehicleModel()<<std::endl;
+    std::cout<<" haveQPSolver : "<<mpc.hasQPSolver()<<std::endl;
+
+    // Init parameters and reference trajectory
+    initializeMPC(mpc);
+
+    // Calculate MPC
+    ADCAckermannLateralCommand ctrl_cmd;
+    MPCTrajectory pred_traj;
+    const auto odom = makeOdometry(pose_zero, default_velocity);
+    bool bcalc = mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj);
+
+    if(bcalc)
+    {
+        std::cout<<" tire: "<<ctrl_cmd.steering_tire_angle<<std::endl;
+        std::cout<<" tire rotation rate: "<<ctrl_cmd.steering_tire_rotation_rate<<std::endl;
+        if((ctrl_cmd.steering_tire_angle == 0.0) && (ctrl_cmd.steering_tire_rotation_rate == 0.0))
+        {
+            std::cout<<" InitializeAndCalculate Test OK"<<std::endl;
+            return;
+        }
+    }
+    else
+    {
+        std::cout<<" mpc calculation fail. "<<std::endl;
+    }
+
+    std::cout<<" InitializeAndCalculate Test Fail."<<std::endl;
+
+}

src/decition/mpccontroller/mpctest.h

@@ -0,0 +1,162 @@
+#ifndef MPCTEST_H
+#define MPCTEST_H
+
+#include "math.h"
+
+#include "mpc_lateral_controller/mpc.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
+
+using namespace autoware::motion::control::mpc_lateral_controller;
+
+class MPCTest
+{
+public:
+    MPCTest();
+
+protected:
+    MPCParam param;
+    // Test inputs
+    MPCTrajectory dummy_straight_trajectory;
+    MPCTrajectory dummy_right_turn_trajectory;
+    iv::ADCSteeringReport neutral_steer;
+    iv::ADCPose pose_zero;
+    double default_velocity = 1.0;
+ //   rclcpp::Logger logger = rclcpp::get_logger("mpc_test_logger");
+
+    // Vehicle model parameters
+    double wheelbase = 2.7;
+    double steer_limit = 1.0;
+    double steer_tau = 0.1;
+    double mass_fl = 600.0;
+    double mass_fr = 600.0;
+    double mass_rl = 600.0;
+    double mass_rr = 600.0;
+    double cf = 155494.663;
+    double cr = 155494.663;
+
+    // Filters parameter
+    double steering_lpf_cutoff_hz = 3.0;
+    double error_deriv_lpf_cutoff_hz = 5.0;
+
+    // Test Parameters
+    double admissible_position_error = 5.0;
+    double admissible_yaw_error_rad = M_PI_2;
+    double steer_lim = 0.610865;      // 35 degrees
+    double steer_rate_lim = 2.61799;  // 150 degrees
+    double ctrl_period = 0.03;
+
+    bool use_steer_prediction = true;
+
+    TrajectoryFilteringParam trajectory_param;
+    MPCTrajectoryPoint makePoint(const double x, const double y, const float vx)
+    {
+        MPCTrajectoryPoint p;
+        p.x = x;
+        p.y = y;
+        p.vx = vx;
+        return p;
+    }
+
+    void SetUp()
+    {
+        param.prediction_horizon = 50;
+        param.prediction_dt = 0.1;
+        param.zero_ff_steer_deg = 0.5;
+        param.input_delay = 0.0;
+        param.acceleration_limit = 2.0;
+        param.velocity_time_constant = 0.3;
+        param.min_prediction_length = 5.0;
+        param.steer_tau = 0.1;
+        param.nominal_weight.lat_error = 1.0;
+        param.nominal_weight.heading_error = 1.0;
+        param.nominal_weight.heading_error_squared_vel = 1.0;
+        param.nominal_weight.terminal_lat_error = 1.0;
+        param.nominal_weight.terminal_heading_error = 0.1;
+        param.low_curvature_weight.lat_error = 0.1;
+        param.low_curvature_weight.heading_error = 0.0;
+        param.low_curvature_weight.heading_error_squared_vel = 0.3;
+        param.nominal_weight.steering_input = 1.0;
+        param.nominal_weight.steering_input_squared_vel = 0.25;
+        param.nominal_weight.lat_jerk = 0.0;
+        param.nominal_weight.steer_rate = 0.0;
+        param.nominal_weight.steer_acc = 0.000001;
+        param.low_curvature_weight.steering_input = 1.0;
+        param.low_curvature_weight.steering_input_squared_vel = 0.25;
+        param.low_curvature_weight.lat_jerk = 0.0;
+        param.low_curvature_weight.steer_rate = 0.0;
+        param.low_curvature_weight.steer_acc = 0.000001;
+        param.low_curvature_thresh_curvature = 0.0;
+
+        trajectory_param.traj_resample_dist = 0.1;
+        trajectory_param.path_filter_moving_ave_num = 35;
+        trajectory_param.curvature_smoothing_num_traj = 1;
+        trajectory_param.curvature_smoothing_num_ref_steer = 35;
+        trajectory_param.enable_path_smoothing = true;
+        trajectory_param.extend_trajectory_for_end_yaw_control = true;
+
+        dummy_straight_trajectory.push_back(0,0.0,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(1,0.1,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(2,0.3,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(3,0.6,0.0,0.0,1.0,0,0,0);
+        dummy_straight_trajectory.push_back(4,0.9,0.0,0.0,1.0,0,0,0);
+
+
+        dummy_right_turn_trajectory.push_back(-1.0,-1.0,0.0,0.0,1.0,0,0,0);
+        dummy_right_turn_trajectory.push_back(0.0,0.0,0.0,0.0,1.0,0,0,0);
+        dummy_right_turn_trajectory.push_back(1.0,-1.0,0.0,0.0,1.0,0,0,0);
+        dummy_right_turn_trajectory.push_back(2.0,-2.0,0.0,0.0,1.0,0,0,0);
+
+//        dummy_straight_trajectory.points.push_back(makePoint(0.0, 0.0, 1.0f));
+//        dummy_straight_trajectory.points.push_back(makePoint(1.0, 0.0, 1.0f));
+//        dummy_straight_trajectory.points.push_back(makePoint(2.0, 0.0, 1.0f));
+//        dummy_straight_trajectory.points.push_back(makePoint(3.0, 0.0, 1.0f));
+//        dummy_straight_trajectory.points.push_back(makePoint(4.0, 0.0, 1.0f));
+
+//        dummy_right_turn_trajectory.points.push_back(makePoint(-1.0, -1.0, 1.0f));
+//        dummy_right_turn_trajectory.points.push_back(makePoint(0.0, 0.0, 1.0f));
+//        dummy_right_turn_trajectory.points.push_back(makePoint(1.0, -1.0, 1.0f));
+//        dummy_right_turn_trajectory.points.push_back(makePoint(2.0, -2.0, 1.0f));
+
+        neutral_steer.steering_tire_angle = 0.0;
+        pose_zero.position.x = 0.0;
+        pose_zero.position.y = 0.0;
+    }
+
+    void initializeMPC(MPC & mpc)
+    {
+        mpc.m_param = param;
+        mpc.m_admissible_position_error = admissible_position_error;
+        mpc.m_admissible_yaw_error_rad = admissible_yaw_error_rad;
+        mpc.m_steer_lim = steer_lim;
+        mpc.m_steer_rate_lim_map_by_curvature.emplace_back(0.0, steer_rate_lim);
+        mpc.m_steer_rate_lim_map_by_velocity.emplace_back(0.0, steer_rate_lim);
+        mpc.m_ctrl_period = ctrl_period;
+        mpc.m_use_steer_prediction = use_steer_prediction;
+
+        mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
+        mpc.initializeSteeringPredictor();
+
+
+        // Init trajectory
+        const auto current_kinematics =
+            makeOdometry(pose_zero, 0.0);
+        mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
+    }
+
+    iv::ADCOdometry makeOdometry(const iv::ADCPose & pose, const double velocity)
+    {
+        iv::ADCOdometry odometry;
+        odometry.pose = pose;
+        odometry.twist.linear_x = velocity;
+        return odometry;
+    }
+
+public:
+    void InitializeAndCalculate();
+};
+
+#endif // MPCTEST_H

src/decition/mpccontroller/testmpc.pro

@@ -11,9 +11,9 @@ SOURCES += \
         lowpass_filter.cpp \
         main.cpp \
         mpc.cpp \
-        mpc_lateral_controller.cpp \
         mpc_trajectory.cpp \
         mpc_utils.cpp \
+        mpctest.cpp \
         osqp_interface.cpp \
         qp_solver/qp_solver_osqp.cpp \
         qp_solver/qp_solver_unconstraint_fast.cpp \
@@ -40,7 +40,6 @@ INCLUDEPATH += /opt/ros/humble/include
 HEADERS += \
     mpc_lateral_controller/lowpass_filter.hpp \
     mpc_lateral_controller/mpc.hpp \
-    mpc_lateral_controller/mpc_lateral_controller.hpp \
     mpc_lateral_controller/mpc_trajectory.hpp \
     mpc_lateral_controller/mpc_utils.hpp \
     mpc_lateral_controller/qp_solver/qp_solver_interface.hpp \
@@ -52,6 +51,7 @@ HEADERS += \
     mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp \
     mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp \
     mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp \
+    mpctest.h \
     vehicle_info_util/vehicle_info.hpp \
     vehicle_info_util/vehicle_info_util.hpp
 
@@ -60,3 +60,6 @@ INCLUDEPATH += $$PWD/../../common/include
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
 }
+
+LIBS += -linterpolation
+LIBS += -L/opt/ros/humble/lib -losqp