add mpcontroller, from autoware, changing and compiling, not complete.

src/decition/mpccontroller/.gitignore

@@ -0,0 +1,74 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+CMakeLists.txt.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/decition/mpccontroller/csc_matrix_conv.cpp

@@ -0,0 +1,141 @@
+// Copyright 2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "osqp_interface/csc_matrix_conv.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+#include <exception>
+#include <iostream>
+#include <vector>
+
+namespace autoware
+{
+namespace common
+{
+namespace osqp
+{
+CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat)
+{
+  const size_t elem = static_cast<size_t>(mat.nonZeros());
+  const Eigen::Index rows = mat.rows();
+  const Eigen::Index cols = mat.cols();
+
+  std::vector<c_float> vals;
+  vals.reserve(elem);
+  std::vector<c_int> row_idxs;
+  row_idxs.reserve(elem);
+  std::vector<c_int> col_idxs;
+  col_idxs.reserve(elem);
+
+  // Construct CSC matrix arrays
+  c_float val;
+  c_int elem_count = 0;
+
+  col_idxs.push_back(0);
+
+  for (Eigen::Index j = 0; j < cols; j++) {    // col iteration
+    for (Eigen::Index i = 0; i < rows; i++) {  // row iteration
+      // Get values of nonzero elements
+      val = mat(i, j);
+      if (std::fabs(val) < 1e-9) {
+        continue;
+      }
+      elem_count += 1;
+
+      // Store values
+      vals.push_back(val);
+      row_idxs.push_back(i);
+    }
+
+    col_idxs.push_back(elem_count);
+  }
+
+  CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs};
+
+  return csc_matrix;
+}
+
+CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat)
+{
+  const size_t elem = static_cast<size_t>(mat.nonZeros());
+  const Eigen::Index rows = mat.rows();
+  const Eigen::Index cols = mat.cols();
+
+  if (rows != cols) {
+    throw std::invalid_argument("Matrix must be square (n, n)");
+  }
+
+  std::vector<c_float> vals;
+  vals.reserve(elem);
+  std::vector<c_int> row_idxs;
+  row_idxs.reserve(elem);
+  std::vector<c_int> col_idxs;
+  col_idxs.reserve(elem);
+
+  // Construct CSC matrix arrays
+  c_float val;
+  Eigen::Index trap_last_idx = 0;
+  c_int elem_count = 0;
+
+  col_idxs.push_back(0);
+
+  for (Eigen::Index j = 0; j < cols; j++) {              // col iteration
+    for (Eigen::Index i = 0; i <= trap_last_idx; i++) {  // row iteration
+      // Get values of nonzero elements
+      val = mat(i, j);
+      if (std::fabs(val) < 1e-9) {
+        continue;
+      }
+      elem_count += 1;
+
+      // Store values
+      vals.push_back(val);
+      row_idxs.push_back(i);
+    }
+
+    col_idxs.push_back(elem_count);
+    trap_last_idx += 1;
+  }
+
+  CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs};
+
+  return csc_matrix;
+}
+
+void printCSCMatrix(const CSC_Matrix & csc_mat)
+{
+  std::cout << "[";
+  for (const c_float & val : csc_mat.m_vals) {
+    std::cout << val << ", ";
+  }
+  std::cout << "]\n";
+
+  std::cout << "[";
+  for (const c_int & row : csc_mat.m_row_idxs) {
+    std::cout << row << ", ";
+  }
+  std::cout << "]\n";
+
+  std::cout << "[";
+  for (const c_int & col : csc_mat.m_col_idxs) {
+    std::cout << col << ", ";
+  }
+  std::cout << "]\n";
+}
+
+}  // namespace osqp
+}  // namespace common
+}  // namespace autoware

src/decition/mpccontroller/lowpass_filter.cpp

@@ -0,0 +1,141 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/lowpass_filter.hpp"
+
+#include <vector>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+Butterworth2dFilter::Butterworth2dFilter(double dt, double f_cutoff_hz)
+{
+  initialize(dt, f_cutoff_hz);
+}
+
+Butterworth2dFilter::~Butterworth2dFilter()
+{
+}
+
+void Butterworth2dFilter::initialize(const double & dt, const double & f_cutoff_hz)
+{
+  m_y1 = 0.0;
+  m_y2 = 0.0;
+  m_u2 = 0.0;
+  m_u1 = 0.0;
+
+  // 2d Butterworth low pass filter with bi-linear transformation
+  const double f_sampling_hz = 1.0 / dt;
+  const double xi = 1.0 / std::tan(M_PI * f_cutoff_hz / f_sampling_hz);
+  const double xi_2 = xi * xi;
+  const double q = std::sqrt(2);
+  m_b0 = 1.0 / (1.0 + q * xi + xi_2);
+  m_b1 = 2.0 * m_b0;
+  m_b2 = m_b0;
+  m_a1 = 2.0 * (xi_2 - 1.0) * m_b0;
+  m_a2 = -(1.0 - q * xi + xi_2) * m_b0;
+}
+
+double Butterworth2dFilter::filter(const double & u0)
+{
+  double y0 = m_b2 * m_u2 + m_b1 * m_u1 + m_b0 * u0 + m_a2 * m_y2 + m_a1 * m_y1;
+  m_y2 = m_y1;
+  m_y1 = y0;
+  m_u2 = m_u1;
+  m_u1 = u0;
+  return y0;
+}
+
+void Butterworth2dFilter::filt_vector(const std::vector<double> & t, std::vector<double> & u) const
+{
+  u.resize(t.size());
+  double y1 = t.at(0);
+  double y2 = t.at(0);
+  double u2 = t.at(0);
+  double u1 = t.at(0);
+  double y0 = 0.0;
+  double u0 = 0.0;
+  for (size_t i = 0; i < t.size(); ++i) {
+    u0 = t.at(i);
+    y0 = m_b2 * u2 + m_b1 * u1 + m_b0 * u0 + m_a2 * y2 + m_a1 * y1;
+    y2 = y1;
+    y1 = y0;
+    u2 = u1;
+    u1 = u0;
+    u.at(i) = y0;
+  }
+}
+
+// filtering forward and backward direction
+void Butterworth2dFilter::filtfilt_vector(
+  const std::vector<double> & t, std::vector<double> & u) const
+{
+  std::vector<double> t_fwd(t);
+  std::vector<double> t_rev(t);
+
+  // forward filtering
+  filt_vector(t, t_fwd);
+
+  // backward filtering
+  std::reverse(t_rev.begin(), t_rev.end());
+  filt_vector(t, t_rev);
+  std::reverse(t_rev.begin(), t_rev.end());
+
+  // merge
+  u.clear();
+  for (size_t i = 0; i < t.size(); ++i) {
+    u.push_back((t_fwd[i] + t_rev[i]) * 0.5);
+  }
+}
+
+void Butterworth2dFilter::getCoefficients(std::vector<double> & coeffs) const
+{
+  coeffs.clear();
+  coeffs.push_back(m_a1);
+  coeffs.push_back(m_a2);
+  coeffs.push_back(m_b0);
+  coeffs.push_back(m_b1);
+  coeffs.push_back(m_b2);
+}
+
+namespace MoveAverageFilter
+{
+bool filt_vector(const int num, std::vector<double> & u)
+{
+  if (static_cast<int>(u.size()) < num) {
+    return false;
+  }
+  std::vector<double> filtered_u(u);
+  for (int i = 0; i < static_cast<int>(u.size()); ++i) {
+    double tmp = 0.0;
+    int num_tmp = 0;
+    double count = 0;
+    if (i - num < 0) {
+      num_tmp = i;
+    } else if (i + num > static_cast<int>(u.size()) - 1) {
+      num_tmp = static_cast<int>(u.size()) - i - 1;
+    } else {
+      num_tmp = num;
+    }
+
+    for (int j = -num_tmp; j <= num_tmp; ++j) {
+      tmp += u[static_cast<size_t>(i + j)];
+      ++count;
+    }
+    filtered_u[static_cast<size_t>(i)] = tmp / count;
+  }
+  u = filtered_u;
+  return true;
+}
+}  // namespace MoveAverageFilter
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/main.cpp

@@ -0,0 +1,8 @@
+#include <QCoreApplication>
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    return a.exec();
+}

src/decition/mpccontroller/mpc.cpp

@@ -0,0 +1,809 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/mpc.hpp"
+
+#include "interpolation/linear_interpolation.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "mpc_lateral_controller/mpc_utils.hpp"
+#include "tier4_autoware_utils/math/unit_conversion.hpp"
+
+#include <algorithm>
+#include <limits>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+using tier4_autoware_utils::calcDistance2d;
+using tier4_autoware_utils::normalizeRadian;
+using tier4_autoware_utils::rad2deg;
+
+bool MPC::calculateMPC(
+  const SteeringReport & current_steer, const Odometry & current_kinematics,
+  AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
+  Float32MultiArrayStamped & diagnostic)
+{
+  // since the reference trajectory does not take into account the current velocity of the ego
+  // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics.
+  const auto reference_trajectory =
+    applyVelocityDynamicsFilter(m_reference_trajectory, current_kinematics);
+
+  // get the necessary data
+  const auto [success_data, mpc_data] =
+    getData(reference_trajectory, current_steer, current_kinematics);
+  if (!success_data) {
+    return fail_warn_throttle("fail to get MPC Data. Stop MPC.");
+  }
+
+  // calculate initial state of the error dynamics
+  const auto x0 = getInitialState(mpc_data);
+
+  // apply time delay compensation to the initial state
+  const auto [success_delay, x0_delayed] =
+    updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, x0);
+  if (!success_delay) {
+    return fail_warn_throttle("delay compensation failed. Stop MPC.");
+  }
+
+  // resample reference trajectory with mpc sampling time
+  const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
+  const double prediction_dt =
+    getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_kinematics);
+
+  const auto [success_resample, mpc_resampled_ref_trajectory] =
+    resampleMPCTrajectoryByTime(mpc_start_time, prediction_dt, reference_trajectory);
+  if (!success_resample) {
+    return fail_warn_throttle("trajectory resampling failed. Stop MPC.");
+  }
+
+  // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex
+  const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
+
+  // solve Optimization problem
+  const auto [success_opt, Uex] = executeOptimization(
+    mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
+    current_kinematics.twist.twist.linear.x);
+  if (!success_opt) {
+    return fail_warn_throttle("optimization failed. Stop MPC.");
+  }
+
+  // apply filters for the input limitation and low pass filter
+  const double u_saturated = std::clamp(Uex(0), -m_steer_lim, m_steer_lim);
+  const double u_filtered = m_lpf_steering_cmd.filter(u_saturated);
+
+  // set control command
+  ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
+  ctrl_cmd.steering_tire_rotation_rate = static_cast<float>(calcDesiredSteeringRate(
+    mpc_matrix, x0_delayed, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));
+
+  // save the control command for the steering prediction
+  m_steering_predictor->storeSteerCmd(u_filtered);
+
+  // save input to buffer for delay compensation
+  m_input_buffer.push_back(ctrl_cmd.steering_tire_angle);
+  m_input_buffer.pop_front();
+
+  // save previous input for the mpc rate limit
+  m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev;
+  m_raw_steer_cmd_prev = Uex(0);
+
+  // calculate predicted trajectory
+  predicted_trajectory =
+    calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex);
+
+  // prepare diagnostic message
+  diagnostic =
+    generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
+
+  return true;
+}
+
+Trajectory MPC::calcPredictedTrajectory(
+  const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix,
+  const VectorXd & x0_delayed, const VectorXd & Uex) const
+{
+  const VectorXd Xex = mpc_matrix.Aex * x0_delayed + mpc_matrix.Bex * Uex + mpc_matrix.Wex;
+  MPCTrajectory mpc_predicted_traj;
+  const auto & traj = mpc_resampled_ref_trajectory;
+  for (int i = 0; i < m_param.prediction_horizon; ++i) {
+    const int DIM_X = m_vehicle_model_ptr->getDimX();
+    const double lat_error = Xex(i * DIM_X);
+    const double yaw_error = Xex(i * DIM_X + 1);
+    const double x = traj.x.at(i) - std::sin(traj.yaw.at(i)) * lat_error;
+    const double y = traj.y.at(i) + std::cos(traj.yaw.at(i)) * lat_error;
+    const double z = traj.z.at(i);
+    const double yaw = traj.yaw.at(i) + yaw_error;
+    const double vx = traj.vx.at(i);
+    const double k = traj.k.at(i);
+    const double smooth_k = traj.smooth_k.at(i);
+    const double relative_time = traj.relative_time.at(i);
+    mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time);
+  }
+  return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj);
+}
+
+Float32MultiArrayStamped MPC::generateDiagData(
+  const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
+  const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
+  const Odometry & current_kinematics) const
+{
+  Float32MultiArrayStamped diagnostic;
+
+  // prepare diagnostic message
+  const double nearest_k = reference_trajectory.k.at(mpc_data.nearest_idx);
+  const double nearest_smooth_k = reference_trajectory.smooth_k.at(mpc_data.nearest_idx);
+  const double wb = m_vehicle_model_ptr->getWheelbase();
+  const double current_velocity = current_kinematics.twist.twist.linear.x;
+  const double wz_predicted = current_velocity * std::tan(mpc_data.predicted_steer) / wb;
+  const double wz_measured = current_velocity * std::tan(mpc_data.steer) / wb;
+  const double wz_command = current_velocity * std::tan(ctrl_cmd.steering_tire_angle) / wb;
+
+  typedef decltype(diagnostic.data)::value_type DiagnosticValueType;
+  const auto append_diag = [&](const auto & val) -> void {
+    diagnostic.data.push_back(static_cast<DiagnosticValueType>(val));
+  };
+  append_diag(ctrl_cmd.steering_tire_angle);      // [0] final steering command (MPC + LPF)
+  append_diag(Uex(0));                            // [1] mpc calculation result
+  append_diag(mpc_matrix.Uref_ex(0));             // [2] feed-forward steering value
+  append_diag(std::atan(nearest_smooth_k * wb));  // [3] feed-forward steering value raw
+  append_diag(mpc_data.steer);                    // [4] current steering angle
+  append_diag(mpc_data.lateral_err);              // [5] lateral error
+  append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation));  // [6] current_pose yaw
+  append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation));         // [7] nearest_pose yaw
+  append_diag(mpc_data.yaw_err);                                       // [8] yaw error
+  append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx));       // [9] reference velocity
+  append_diag(current_velocity);                                       // [10] measured velocity
+  append_diag(wz_command);                           // [11] angular velocity from steer command
+  append_diag(wz_measured);                          // [12] angular velocity from measured steer
+  append_diag(current_velocity * nearest_smooth_k);  // [13] angular velocity from path curvature
+  append_diag(nearest_smooth_k);          // [14] nearest path curvature (used for feed-forward)
+  append_diag(nearest_k);                 // [15] nearest path curvature (not smoothed)
+  append_diag(mpc_data.predicted_steer);  // [16] predicted steer
+  append_diag(wz_predicted);              // [17] angular velocity from predicted steer
+
+  return diagnostic;
+}
+
+void MPC::setReferenceTrajectory(
+  const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
+  const Odometry & current_kinematics)
+{
+  const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+    trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
+    ego_nearest_yaw_threshold);
+  const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment(
+    trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);
+
+  const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
+
+  // resampling
+  const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
+    mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
+  if (!success_resample) {
+    warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
+    return;
+  }
+
+  const auto is_forward_shift =
+    motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints());
+
+  // if driving direction is unknown, use previous value
+  m_is_forward_shift = is_forward_shift ? is_forward_shift.get() : m_is_forward_shift;
+
+  // path smoothing
+  MPCTrajectory mpc_traj_smoothed = mpc_traj_resampled;  // smooth filtered trajectory
+  const int mpc_traj_resampled_size = static_cast<int>(mpc_traj_resampled.size());
+  if (
+    param.enable_path_smoothing && mpc_traj_resampled_size > 2 * param.path_filter_moving_ave_num) {
+    using MoveAverageFilter::filt_vector;
+    if (
+      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.x) ||
+      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.y) ||
+      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.yaw) ||
+      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.vx)) {
+      RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering.");
+      mpc_traj_smoothed = mpc_traj_resampled;
+    }
+  }
+
+  /*
+   * Extend terminal points
+   * Note: The current MPC does not properly take into account the attitude angle at the end of the
+   * path. By extending the end of the path in the attitude direction, the MPC can consider the
+   * attitude angle well, resulting in improved control performance. If the trajectory is
+   * well-defined considering the end point attitude angle, this feature is not necessary.
+   */
+  if (param.extend_trajectory_for_end_yaw_control) {
+    MPCUtils::extendTrajectoryInYawDirection(
+      mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
+  }
+
+  // calculate yaw angle
+  MPCUtils::calcTrajectoryYawFromXY(mpc_traj_smoothed, m_is_forward_shift);
+  MPCUtils::convertEulerAngleToMonotonic(mpc_traj_smoothed.yaw);
+
+  // calculate curvature
+  MPCUtils::calcTrajectoryCurvature(
+    param.curvature_smoothing_num_traj, param.curvature_smoothing_num_ref_steer, mpc_traj_smoothed);
+
+  // stop velocity at a terminal point
+  mpc_traj_smoothed.vx.back() = 0.0;
+
+  // add a extra point on back with extended time to make the mpc stable.
+  auto last_point = mpc_traj_smoothed.back();
+  last_point.relative_time += 100.0;  // extra time to prevent mpc calc failure due to short time
+  last_point.vx = 0.0;                // stop velocity at a terminal point
+  mpc_traj_smoothed.push_back(last_point);
+
+  if (!mpc_traj_smoothed.size()) {
+    RCLCPP_DEBUG(m_logger, "path callback: trajectory size is undesired.");
+    return;
+  }
+
+  m_reference_trajectory = mpc_traj_smoothed;
+}
+
+void MPC::resetPrevResult(const SteeringReport & current_steer)
+{
+  // Consider limit. The prev value larger than limitation brakes the optimization constraint and
+  // results in optimization failure.
+  const float steer_lim_f = static_cast<float>(m_steer_lim);
+  m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
+  m_raw_steer_cmd_pprev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
+}
+
+std::pair<bool, MPCData> MPC::getData(
+  const MPCTrajectory & traj, const SteeringReport & current_steer,
+  const Odometry & current_kinematics)
+{
+  const auto current_pose = current_kinematics.pose.pose;
+
+  MPCData data;
+  if (!MPCUtils::calcNearestPoseInterp(
+        traj, current_pose, &(data.nearest_pose), &(data.nearest_idx), &(data.nearest_time),
+        ego_nearest_dist_threshold, ego_nearest_yaw_threshold)) {
+    warn_throttle("calculateMPC: error in calculating nearest pose. stop mpc.");
+    return {false, MPCData{}};
+  }
+
+  // get data
+  data.steer = static_cast<double>(current_steer.steering_tire_angle);
+  data.lateral_err = MPCUtils::calcLateralError(current_pose, data.nearest_pose);
+  data.yaw_err = normalizeRadian(
+    tf2::getYaw(current_pose.orientation) - tf2::getYaw(data.nearest_pose.orientation));
+
+  // get predicted steer
+  data.predicted_steer = m_steering_predictor->calcSteerPrediction();
+
+  // check error limit
+  const double dist_err = calcDistance2d(current_pose, data.nearest_pose);
+  if (dist_err > m_admissible_position_error) {
+    warn_throttle("Too large position error: %fm > %fm", dist_err, m_admissible_position_error);
+    return {false, MPCData{}};
+  }
+
+  // check yaw error limit
+  if (std::fabs(data.yaw_err) > m_admissible_yaw_error_rad) {
+    warn_throttle("Too large yaw error: %f > %f", data.yaw_err, m_admissible_yaw_error_rad);
+    return {false, MPCData{}};
+  }
+
+  // check trajectory time length
+  const double max_prediction_time =
+    m_param.min_prediction_length / static_cast<double>(m_param.prediction_horizon - 1);
+  auto end_time = data.nearest_time + m_param.input_delay + m_ctrl_period + max_prediction_time;
+  if (end_time > traj.relative_time.back()) {
+    warn_throttle("path is too short for prediction.");
+    return {false, MPCData{}};
+  }
+  return {true, data};
+}
+
+std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
+  const double ts, const double prediction_dt, const MPCTrajectory & input) const
+{
+  MPCTrajectory output;
+  std::vector<double> mpc_time_v;
+  for (double i = 0; i < static_cast<double>(m_param.prediction_horizon); ++i) {
+    mpc_time_v.push_back(ts + i * prediction_dt);
+  }
+  if (!MPCUtils::linearInterpMPCTrajectory(input.relative_time, input, mpc_time_v, output)) {
+    warn_throttle("calculateMPC: mpc resample error. stop mpc calculation. check code!");
+    return {false, {}};
+  }
+  return {true, output};
+}
+
+VectorXd MPC::getInitialState(const MPCData & data)
+{
+  const int DIM_X = m_vehicle_model_ptr->getDimX();
+  VectorXd x0 = VectorXd::Zero(DIM_X);
+
+  const auto & lat_err = data.lateral_err;
+  const auto & steer = m_use_steer_prediction ? data.predicted_steer : data.steer;
+  const auto & yaw_err = data.yaw_err;
+
+  const auto vehicle_model = m_vehicle_model_ptr->modelName();
+  if (vehicle_model == "kinematics") {
+    x0 << lat_err, yaw_err, steer;
+  } else if (vehicle_model == "kinematics_no_delay") {
+    x0 << lat_err, yaw_err;
+  } else if (vehicle_model == "dynamics") {
+    double dlat = (lat_err - m_lateral_error_prev) / m_ctrl_period;
+    double dyaw = (yaw_err - m_yaw_error_prev) / m_ctrl_period;
+    m_lateral_error_prev = lat_err;
+    m_yaw_error_prev = yaw_err;
+    dlat = m_lpf_lateral_error.filter(dlat);
+    dyaw = m_lpf_yaw_error.filter(dyaw);
+    x0 << lat_err, dlat, yaw_err, dyaw;
+    RCLCPP_DEBUG(m_logger, "(before lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
+    RCLCPP_DEBUG(m_logger, "(after lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
+  } else {
+    RCLCPP_ERROR(m_logger, "vehicle_model_type is undefined");
+  }
+  return x0;
+}
+
+std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
+  const MPCTrajectory & traj, const double & start_time, const VectorXd & x0_orig)
+{
+  const int DIM_X = m_vehicle_model_ptr->getDimX();
+  const int DIM_U = m_vehicle_model_ptr->getDimU();
+  const int DIM_Y = m_vehicle_model_ptr->getDimY();
+
+  MatrixXd Ad(DIM_X, DIM_X);
+  MatrixXd Bd(DIM_X, DIM_U);
+  MatrixXd Wd(DIM_X, 1);
+  MatrixXd Cd(DIM_Y, DIM_X);
+
+  MatrixXd x_curr = x0_orig;
+  double mpc_curr_time = start_time;
+  for (size_t i = 0; i < m_input_buffer.size(); ++i) {
+    double k, v = 0.0;
+    try {
+      k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time);
+      v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time);
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what());
+      return {false, {}};
+    }
+
+    // get discrete state matrix A, B, C, W
+    m_vehicle_model_ptr->setVelocity(v);
+    m_vehicle_model_ptr->setCurvature(k);
+    m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, m_ctrl_period);
+    MatrixXd ud = MatrixXd::Zero(DIM_U, 1);
+    ud(0, 0) = m_input_buffer.at(i);  // for steering input delay
+    x_curr = Ad * x_curr + Bd * ud + Wd;
+    mpc_curr_time += m_ctrl_period;
+  }
+  return {true, x_curr};
+}
+
+MPCTrajectory MPC::applyVelocityDynamicsFilter(
+  const MPCTrajectory & input, const Odometry & current_kinematics) const
+{
+  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
+  if (autoware_traj.points.empty()) {
+    return input;
+  }
+
+  const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+    autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
+    ego_nearest_yaw_threshold);
+
+  MPCTrajectory output = input;
+  MPCUtils::dynamicSmoothingVelocity(
+    nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit,
+    m_param.velocity_time_constant, output);
+
+  auto last_point = output.back();
+  last_point.relative_time += 100.0;  // extra time to prevent mpc calc failure due to short time
+  last_point.vx = 0.0;                // stop velocity at a terminal point
+  output.push_back(last_point);
+  return output;
+}
+
+/*
+ * predict equation: Xec = Aex * x0 + Bex * Uex + Wex
+ * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex
+ * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
+ */
+MPCMatrix MPC::generateMPCMatrix(
+  const MPCTrajectory & reference_trajectory, const double prediction_dt)
+{
+  const int N = m_param.prediction_horizon;
+  const double DT = prediction_dt;
+  const int DIM_X = m_vehicle_model_ptr->getDimX();
+  const int DIM_U = m_vehicle_model_ptr->getDimU();
+  const int DIM_Y = m_vehicle_model_ptr->getDimY();
+
+  MPCMatrix m;
+  m.Aex = MatrixXd::Zero(DIM_X * N, DIM_X);
+  m.Bex = MatrixXd::Zero(DIM_X * N, DIM_U * N);
+  m.Wex = MatrixXd::Zero(DIM_X * N, 1);
+  m.Cex = MatrixXd::Zero(DIM_Y * N, DIM_X * N);
+  m.Qex = MatrixXd::Zero(DIM_Y * N, DIM_Y * N);
+  m.R1ex = MatrixXd::Zero(DIM_U * N, DIM_U * N);
+  m.R2ex = MatrixXd::Zero(DIM_U * N, DIM_U * N);
+  m.Uref_ex = MatrixXd::Zero(DIM_U * N, 1);
+
+  // weight matrix depends on the vehicle model
+  MatrixXd Q = MatrixXd::Zero(DIM_Y, DIM_Y);
+  MatrixXd R = MatrixXd::Zero(DIM_U, DIM_U);
+  MatrixXd Q_adaptive = MatrixXd::Zero(DIM_Y, DIM_Y);
+  MatrixXd R_adaptive = MatrixXd::Zero(DIM_U, DIM_U);
+
+  MatrixXd Ad(DIM_X, DIM_X);
+  MatrixXd Bd(DIM_X, DIM_U);
+  MatrixXd Wd(DIM_X, 1);
+  MatrixXd Cd(DIM_Y, DIM_X);
+  MatrixXd Uref(DIM_U, 1);
+
+  const double sign_vx = m_is_forward_shift ? 1 : -1;
+
+  // predict dynamics for N times
+  for (int i = 0; i < N; ++i) {
+    const double ref_vx = reference_trajectory.vx.at(i);
+    const double ref_vx_squared = ref_vx * ref_vx;
+
+    const double ref_k = reference_trajectory.k.at(i) * sign_vx;
+    const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx;
+
+    // get discrete state matrix A, B, C, W
+    m_vehicle_model_ptr->setVelocity(ref_vx);
+    m_vehicle_model_ptr->setCurvature(ref_k);
+    m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT);
+
+    Q = MatrixXd::Zero(DIM_Y, DIM_Y);
+    R = MatrixXd::Zero(DIM_U, DIM_U);
+    const auto mpc_weight = getWeight(ref_k);
+    Q(0, 0) = mpc_weight.lat_error;
+    Q(1, 1) = mpc_weight.heading_error;
+    R(0, 0) = mpc_weight.steering_input;
+
+    Q_adaptive = Q;
+    R_adaptive = R;
+    if (i == N - 1) {
+      Q_adaptive(0, 0) = m_param.nominal_weight.terminal_lat_error;
+      Q_adaptive(1, 1) = m_param.nominal_weight.terminal_heading_error;
+    }
+    Q_adaptive(1, 1) += ref_vx_squared * mpc_weight.heading_error_squared_vel;
+    R_adaptive(0, 0) += ref_vx_squared * mpc_weight.steering_input_squared_vel;
+
+    // update mpc matrix
+    int idx_x_i = i * DIM_X;
+    int idx_x_i_prev = (i - 1) * DIM_X;
+    int idx_u_i = i * DIM_U;
+    int idx_y_i = i * DIM_Y;
+    if (i == 0) {
+      m.Aex.block(0, 0, DIM_X, DIM_X) = Ad;
+      m.Bex.block(0, 0, DIM_X, DIM_U) = Bd;
+      m.Wex.block(0, 0, DIM_X, 1) = Wd;
+    } else {
+      m.Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * m.Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X);
+      for (int j = 0; j < i; ++j) {
+        int idx_u_j = j * DIM_U;
+        m.Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) =
+          Ad * m.Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U);
+      }
+      m.Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * m.Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd;
+    }
+    m.Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd;
+    m.Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd;
+    m.Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive;
+    m.R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive;
+
+    // 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)) < tier4_autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) {
+      Uref(0, 0) = 0.0;  // ignore curvature noise
+    }
+    m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref;
+  }
+
+  // add lateral jerk : weight for (v * {u(i) - u(i-1)} )^2
+  for (int i = 0; i < N - 1; ++i) {
+    const double ref_vx = reference_trajectory.vx.at(i);
+    const double ref_k = reference_trajectory.k.at(i) * sign_vx;
+    const double j = ref_vx * ref_vx * getWeight(ref_k).lat_jerk / (DT * DT);
+    const Eigen::Matrix2d J = (Eigen::Matrix2d() << j, -j, -j, j).finished();
+    m.R2ex.block(i, i, 2, 2) += J;
+  }
+
+  addSteerWeightR(prediction_dt, m.R1ex);
+
+  return m;
+}
+
+/*
+ * solve quadratic optimization.
+ * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex
+ *                , Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
+ * constraint matrix : lb < U < ub, lbA < A*U < ubA
+ * current considered constraint
+ *  - steering limit
+ *  - steering rate limit
+ *
+ * (1)lb < u < ub && (2)lbA < Au < ubA --> (3)[lb, lbA] < [I, A]u < [ub, ubA]
+ * (1)lb < u < ub ...
+ * [-u_lim] < [ u0 ] < [u_lim]
+ * [-u_lim] < [ u1 ] < [u_lim]
+ *              ~~~
+ * [-u_lim] < [ uN ] < [u_lim] (*N... DIM_U)
+ * (2)lbA < Au < ubA ...
+ * [prev_u0 - au_lim*ctp] < [   u0  ] < [prev_u0 + au_lim*ctp] (*ctp ... ctrl_period)
+ * [    -au_lim * dt    ] < [u1 - u0] < [     au_lim * dt    ]
+ * [    -au_lim * dt    ] < [u2 - u1] < [     au_lim * dt    ]
+ *                            ~~~
+ * [    -au_lim * dt    ] < [uN-uN-1] < [     au_lim * dt    ] (*N... DIM_U)
+ */
+std::pair<bool, VectorXd> MPC::executeOptimization(
+  const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
+  const double current_velocity)
+{
+  VectorXd Uex;
+
+  if (!isValid(m)) {
+    warn_throttle("model matrix is invalid. stop MPC.");
+    return {false, {}};
+  }
+
+  const int DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU();
+
+  // cost function: 1/2 * Uex' * H * Uex + f' * Uex,  H = B' * C' * Q * C * B + R
+  const MatrixXd CB = m.Cex * m.Bex;
+  const MatrixXd QCB = m.Qex * CB;
+  // MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; // This calculation is heavy. looking for
+  // a good way.  //NOLINT
+  MatrixXd H = MatrixXd::Zero(DIM_U_N, DIM_U_N);
+  H.triangularView<Eigen::Upper>() = CB.transpose() * QCB;
+  H.triangularView<Eigen::Upper>() += m.R1ex + m.R2ex;
+  H.triangularView<Eigen::Lower>() = H.transpose();
+  MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex;
+  addSteerWeightF(prediction_dt, f);
+
+  MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N);
+  for (int i = 1; i < DIM_U_N; i++) {
+    A(i, i - 1) = -1.0;
+  }
+
+  // steering angle limit
+  VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim);  // min steering angle
+  VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // max steering angle
+
+  // steering angle rate limit
+  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
+  VectorXd ubA = steer_rate_limits * prediction_dt;
+  VectorXd lbA = -steer_rate_limits * prediction_dt;
+  ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period;
+  lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period;
+
+  auto t_start = std::chrono::system_clock::now();
+  bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex);
+  auto t_end = std::chrono::system_clock::now();
+  if (!solve_result) {
+    warn_throttle("qp solver error");
+    return {false, {}};
+  }
+
+  {
+    auto t = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
+    RCLCPP_DEBUG(m_logger, "qp solver calculation time = %ld [ms]", t);
+  }
+
+  if (Uex.array().isNaN().any()) {
+    warn_throttle("model Uex includes NaN, stop MPC.");
+    return {false, {}};
+  }
+  return {true, Uex};
+}
+
+void MPC::addSteerWeightR(const double prediction_dt, MatrixXd & R) const
+{
+  const int N = m_param.prediction_horizon;
+  const double DT = prediction_dt;
+
+  // add steering rate : weight for (u(i) - u(i-1) / dt )^2
+  {
+    const double steer_rate_r = m_param.nominal_weight.steer_rate / (DT * DT);
+    const Eigen::Matrix2d D = steer_rate_r * (Eigen::Matrix2d() << 1.0, -1.0, -1.0, 1.0).finished();
+    for (int i = 0; i < N - 1; ++i) {
+      R.block(i, i, 2, 2) += D;
+    }
+    if (N > 1) {
+      // steer rate i = 0
+      R(0, 0) += m_param.nominal_weight.steer_rate / (m_ctrl_period * m_ctrl_period);
+    }
+  }
+
+  // add steering acceleration : weight for { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2
+  {
+    const double w = m_param.nominal_weight.steer_acc;
+    const double steer_acc_r = w / std::pow(DT, 4);
+    const double steer_acc_r_cp1 = w / (std::pow(DT, 3) * m_ctrl_period);
+    const double steer_acc_r_cp2 = w / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2));
+    const double steer_acc_r_cp4 = w / std::pow(m_ctrl_period, 4);
+    const Eigen::Matrix3d D =
+      steer_acc_r *
+      (Eigen::Matrix3d() << 1.0, -2.0, 1.0, -2.0, 4.0, -2.0, 1.0, -2.0, 1.0).finished();
+    for (int i = 1; i < N - 1; ++i) {
+      R.block(i - 1, i - 1, 3, 3) += D;
+    }
+    if (N > 1) {
+      // steer acc i = 1
+      R(0, 0) += steer_acc_r * 1.0 + steer_acc_r_cp2 * 1.0 + steer_acc_r_cp1 * 2.0;
+      R(1, 0) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0;
+      R(0, 1) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0;
+      R(1, 1) += steer_acc_r * 1.0;
+      // steer acc i = 0
+      R(0, 0) += steer_acc_r_cp4 * 1.0;
+    }
+  }
+}
+
+void MPC::addSteerWeightF(const double prediction_dt, MatrixXd & f) const
+{
+  if (f.rows() < 2) {
+    return;
+  }
+
+  const double DT = prediction_dt;
+
+  // steer rate for i = 0
+  f(0, 0) += -2.0 * m_param.nominal_weight.steer_rate / (std::pow(DT, 2)) * 0.5;
+
+  // const double steer_acc_r = m_param.weight_steer_acc / std::pow(DT, 4);
+  const double steer_acc_r_cp1 =
+    m_param.nominal_weight.steer_acc / (std::pow(DT, 3) * m_ctrl_period);
+  const double steer_acc_r_cp2 =
+    m_param.nominal_weight.steer_acc / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2));
+  const double steer_acc_r_cp4 = m_param.nominal_weight.steer_acc / std::pow(m_ctrl_period, 4);
+
+  // steer acc  i = 0
+  f(0, 0) += ((-2.0 * m_raw_steer_cmd_prev + m_raw_steer_cmd_pprev) * steer_acc_r_cp4) * 0.5;
+
+  // steer acc for i = 1
+  f(0, 0) += (-2.0 * m_raw_steer_cmd_prev * (steer_acc_r_cp1 + steer_acc_r_cp2)) * 0.5;
+  f(0, 1) += (2.0 * m_raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5;
+}
+
+double MPC::getPredictionDeltaTime(
+  const double start_time, const MPCTrajectory & input, const Odometry & current_kinematics) const
+{
+  // Calculate the time min_prediction_length ahead from current_pose
+  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
+  const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
+    ego_nearest_yaw_threshold);
+  double sum_dist = 0;
+  const double target_time = [&]() {
+    const double t_ext = 100.0;  // extra time to prevent mpc calculation failure due to short time
+    for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) {
+      const double segment_dist = MPCUtils::calcDistance2d(input, i, i - 1);
+      sum_dist += segment_dist;
+      if (m_param.min_prediction_length < sum_dist) {
+        const double prev_sum_dist = sum_dist - segment_dist;
+        const double ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist;
+        const double relative_time_at_i = i == input.relative_time.size() - 1
+                                            ? input.relative_time.at(i) - t_ext
+                                            : input.relative_time.at(i);
+        return input.relative_time.at(i - 1) +
+               (relative_time_at_i - input.relative_time.at(i - 1)) * ratio;
+      }
+    }
+    return input.relative_time.back() - t_ext;
+  }();
+
+  // Calculate delta time for min_prediction_length
+  const double dt =
+    (target_time - start_time) / static_cast<double>(m_param.prediction_horizon - 1);
+
+  return std::max(dt, m_param.prediction_dt);
+}
+
+double MPC::calcDesiredSteeringRate(
+  const MPCMatrix & mpc_matrix, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered,
+  const float current_steer, const double predict_dt) const
+{
+  if (m_vehicle_model_ptr->modelName() != "kinematics") {
+    // not supported yet. Use old implementation.
+    return (u_filtered - current_steer) / predict_dt;
+  }
+
+  // calculate predicted states to get the steering motion
+  const auto & m = mpc_matrix;
+  const MatrixXd Xex = m.Aex * x0 + m.Bex * Uex + m.Wex;
+
+  const size_t STEER_IDX = 2;  // for kinematics model
+
+  const auto steer_0 = x0(STEER_IDX, 0);
+  const auto steer_1 = Xex(STEER_IDX, 0);
+
+  const auto steer_rate = (steer_1 - steer_0) / predict_dt;
+
+  return steer_rate;
+}
+
+VectorXd MPC::calcSteerRateLimitOnTrajectory(
+  const MPCTrajectory & trajectory, const double current_velocity) const
+{
+  const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
+    std::vector<double> reference, limits;
+    for (const auto & p : steer_rate_limit_map) {
+      reference.push_back(p.first);
+      limits.push_back(p.second);
+    }
+
+    // If the speed is out of range of the reference, apply zero-order hold.
+    if (current <= reference.front()) {
+      return limits.front();
+    }
+    if (current >= reference.back()) {
+      return limits.back();
+    }
+
+    // Apply linear interpolation
+    for (size_t i = 0; i < reference.size() - 1; ++i) {
+      if (reference.at(i) <= current && current <= reference.at(i + 1)) {
+        auto ratio =
+          (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5);
+        ratio = std::clamp(ratio, 0.0, 1.0);
+        const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i));
+        return interp;
+      }
+    }
+
+    std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command "
+                 "filter is not working. Please check the code."
+              << std::endl;
+    return reference.back();
+  };
+
+  // when the vehicle is stopped, no steering rate limit.
+  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
+  if (is_vehicle_stopped) {
+    return VectorXd::Zero(m_param.prediction_horizon);
+  }
+
+  // calculate steering rate limit
+  VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon);
+  for (int i = 0; i < m_param.prediction_horizon; ++i) {
+    const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i));
+    const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i));
+    steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity);
+  }
+
+  return steer_rate_limits;
+}
+
+bool MPC::isValid(const MPCMatrix & m) const
+{
+  if (
+    m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() ||
+    m.Wex.array().isNaN().any() || m.Qex.array().isNaN().any() || m.R1ex.array().isNaN().any() ||
+    m.R2ex.array().isNaN().any() || m.Uref_ex.array().isNaN().any()) {
+    return false;
+  }
+
+  if (
+    m.Aex.array().isInf().any() || m.Bex.array().isInf().any() || m.Cex.array().isInf().any() ||
+    m.Wex.array().isInf().any() || m.Qex.array().isInf().any() || m.R1ex.array().isInf().any() ||
+    m.R2ex.array().isInf().any() || m.Uref_ex.array().isInf().any()) {
+    return false;
+  }
+
+  return true;
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/mpc_lateral_controller.cpp

@@ -0,0 +1,637 @@
+// Copyright 2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/mpc_lateral_controller.hpp"
+
+#include "motion_utils/trajectory/trajectory.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"
+#include "tf2/utils.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "vehicle_info_util/vehicle_info_util.hpp"
+
+#include <algorithm>
+#include <deque>
+#include <limits>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+MpcLateralController::MpcLateralController(rclcpp::Node & node)
+: clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller"))
+{
+  const auto dp_int = [&](const std::string & s) { return node.declare_parameter<int>(s); };
+  const auto dp_bool = [&](const std::string & s) { return node.declare_parameter<bool>(s); };
+  const auto dp_double = [&](const std::string & s) { return node.declare_parameter<double>(s); };
+
+  m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double();
+
+  auto & p_filt = m_trajectory_filtering_param;
+  p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing");
+  p_filt.path_filter_moving_ave_num = dp_int("path_filter_moving_ave_num");
+  p_filt.curvature_smoothing_num_traj = dp_int("curvature_smoothing_num_traj");
+  p_filt.curvature_smoothing_num_ref_steer = dp_int("curvature_smoothing_num_ref_steer");
+  p_filt.traj_resample_dist = dp_double("traj_resample_dist");
+  p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control");
+
+  m_mpc.m_admissible_position_error = dp_double("admissible_position_error");
+  m_mpc.m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad");
+  m_mpc.m_use_steer_prediction = dp_bool("use_steer_prediction");
+  m_mpc.m_param.steer_tau = dp_double("vehicle_model_steer_tau");
+
+  /* stop state parameters */
+  m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed");
+  m_stop_state_entry_target_speed = dp_double("stop_state_entry_target_speed");
+  m_converged_steer_rad = dp_double("converged_steer_rad");
+  m_keep_steer_control_until_converged = dp_bool("keep_steer_control_until_converged");
+  m_new_traj_duration_time = dp_double("new_traj_duration_time");            // [s]
+  m_new_traj_end_dist = dp_double("new_traj_end_dist");                      // [m]
+  m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps");  // [rad/s]
+
+  /* mpc parameters */
+  const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
+  const double wheelbase = vehicle_info.wheel_base_m;
+  constexpr double deg2rad = static_cast<double>(M_PI) / 180.0;
+  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");
+  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),
+      steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad);
+  }
+
+  // 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");
+  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);
+  m_mpc.setVehicleModel(vehicle_model_ptr);
+
+  /* QP solver setup */
+  m_mpc.setVehicleModel(vehicle_model_ptr);
+  auto qpsolver_ptr = createQPSolverInterface(node);
+  m_mpc.setQPSolver(qpsolver_ptr);
+
+  /* delay compensation */
+  {
+    const double delay_tmp = dp_double("input_delay");
+    const double delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period);
+    m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period;
+    m_mpc.m_input_buffer = std::deque<double>(static_cast<size_t>(delay_step), 0.0);
+  }
+
+  /* steering offset compensation */
+  enable_auto_steering_offset_removal_ =
+    dp_bool("steering_offset.enable_auto_steering_offset_removal");
+  steering_offset_ = createSteerOffsetEstimator(wheelbase, node);
+
+  /* initialize low-pass filter */
+  {
+    const double steering_lpf_cutoff_hz = dp_double("steering_lpf_cutoff_hz");
+    const double error_deriv_lpf_cutoff_hz = dp_double("error_deriv_lpf_cutoff_hz");
+    m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
+  }
+
+  // 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);
+  };
+  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);
+
+  declareMPCparameters(node);
+
+  /* get parameter updates */
+  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_);
+}
+
+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> 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<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<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_;
+}
+
+trajectory_follower::LateralOutput MpcLateralController::run(
+  trajectory_follower::InputData const & input_data)
+{
+  // set input data
+  setTrajectory(input_data.current_trajectory, input_data.current_odometry);
+
+  m_current_kinematic_state = input_data.current_odometry;
+  m_current_steering = input_data.current_steering;
+  if (enable_auto_steering_offset_removal_) {
+    m_current_steering.steering_tire_angle -= steering_offset_->getOffset();
+  }
+
+  AckermannLateralCommand ctrl_cmd;
+  Trajectory predicted_traj;
+  Float32MultiArrayStamped debug_values;
+
+  if (!m_is_ctrl_cmd_prev_initialized) {
+    m_ctrl_cmd_prev = getInitialControlCommand();
+    m_is_ctrl_cmd_prev_initialized = true;
+  }
+
+  const bool is_mpc_solved = m_mpc.calculateMPC(
+    m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);
+
+  // reset previous MPC result
+  // Note: When a large deviation from the trajectory occurs, the optimization stops and
+  // the vehicle will return to the path by re-planning the trajectory or external operation.
+  // After the recovery, the previous value of the optimization may deviate greatly from
+  // the actual steer angle, and it may make the optimization result unstable.
+  if (!is_mpc_solved) {
+    m_mpc.resetPrevResult(m_current_steering);
+  } else {
+    setSteeringToHistory(ctrl_cmd);
+  }
+
+  if (enable_auto_steering_offset_removal_) {
+    steering_offset_->updateOffset(
+      m_current_kinematic_state.twist.twist,
+      input_data.current_steering.steering_tire_angle);  // use unbiased steering
+    ctrl_cmd.steering_tire_angle += steering_offset_->getOffset();
+  }
+
+  publishPredictedTraj(predicted_traj);
+  publishDebugValues(debug_values);
+
+  const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
+    trajectory_follower::LateralOutput output;
+    output.control_cmd = createCtrlCmdMsg(cmd);
+    // To be sure current steering of the vehicle is desired steering angle, we need to check
+    // following conditions.
+    // 1. At the last loop, mpc should be solved because command should be optimized output.
+    // 2. The mpc should be converged.
+    // 3. The steer angle should be converged.
+    output.sync_data.is_steer_converged =
+      is_mpc_solved && isMpcConverged() && isSteerConverged(cmd);
+
+    return output;
+  };
+
+  if (isStoppedState()) {
+    // Reset input buffer
+    for (auto & value : m_mpc.m_input_buffer) {
+      value = m_ctrl_cmd_prev.steering_tire_angle;
+    }
+    // Use previous command value as previous raw steer command
+    m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
+    return createLateralOutput(m_ctrl_cmd_prev, false);
+  }
+
+  if (!is_mpc_solved) {
+    warn_throttle("MPC is not solved. publish 0 velocity.");
+    ctrl_cmd = getStopControlCommand();
+  }
+
+  m_ctrl_cmd_prev = ctrl_cmd;
+  return createLateralOutput(ctrl_cmd, is_mpc_solved);
+}
+
+bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) const
+{
+  // wait for a while to propagate the trajectory shape to the output command when the trajectory
+  // shape is changed.
+  if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) {
+    RCLCPP_DEBUG(logger_, "trajectory shaped is changed");
+    return false;
+  }
+
+  const bool is_converged =
+    std::abs(cmd.steering_tire_angle - m_current_steering.steering_tire_angle) <
+    static_cast<float>(m_converged_steer_rad);
+
+  return is_converged;
+}
+
+bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data)
+{
+  setTrajectory(input_data.current_trajectory, input_data.current_odometry);
+  m_current_kinematic_state = input_data.current_odometry;
+  m_current_steering = input_data.current_steering;
+
+  if (!m_mpc.hasVehicleModel()) {
+    info_throttle("MPC does not have a vehicle model");
+    return false;
+  }
+  if (!m_mpc.hasQPSolver()) {
+    info_throttle("MPC does not have a QP solver");
+    return false;
+  }
+  if (m_mpc.m_reference_trajectory.empty()) {
+    info_throttle("trajectory size is zero.");
+    return false;
+  }
+
+  return true;
+}
+
+void MpcLateralController::setTrajectory(
+  const Trajectory & msg, const Odometry & current_kinematics)
+{
+  m_current_trajectory = msg;
+
+  if (msg.points.size() < 3) {
+    RCLCPP_DEBUG(logger_, "received path size is < 3, not enough.");
+    return;
+  }
+
+  if (!isValidTrajectory(msg)) {
+    RCLCPP_ERROR(logger_, "Trajectory is invalid!! stop computing.");
+    return;
+  }
+
+  m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics);
+
+  // update trajectory buffer to check the trajectory shape change.
+  m_trajectory_buffer.push_back(m_current_trajectory);
+  while (rclcpp::ok()) {
+    const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) -
+                           rclcpp::Time(m_trajectory_buffer.front().header.stamp);
+
+    const double first_trajectory_duration_time = 5.0;
+    const double duration_time =
+      m_has_received_first_trajectory ? m_new_traj_duration_time : first_trajectory_duration_time;
+    if (time_diff.seconds() < duration_time) {
+      m_has_received_first_trajectory = true;
+      break;
+    }
+    m_trajectory_buffer.pop_front();
+  }
+}
+
+AckermannLateralCommand MpcLateralController::getStopControlCommand() const
+{
+  AckermannLateralCommand cmd;
+  cmd.steering_tire_angle = static_cast<decltype(cmd.steering_tire_angle)>(m_steer_cmd_prev);
+  cmd.steering_tire_rotation_rate = 0.0;
+  return cmd;
+}
+
+AckermannLateralCommand MpcLateralController::getInitialControlCommand() const
+{
+  AckermannLateralCommand cmd;
+  cmd.steering_tire_angle = m_current_steering.steering_tire_angle;
+  cmd.steering_tire_rotation_rate = 0.0;
+  return cmd;
+}
+
+bool MpcLateralController::isStoppedState() const
+{
+  // If the nearest index is not found, return false
+  if (m_current_trajectory.points.empty()) {
+    return false;
+  }
+
+  // Note: This function used to take into account the distance to the stop line
+  // for the stop state judgement. However, it has been removed since the steering
+  // control was turned off when approaching/exceeding the stop line on a curve or
+  // emergency stop situation and it caused large tracking error.
+  const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold,
+    m_ego_nearest_yaw_threshold);
+
+  const double current_vel = m_current_kinematic_state.twist.twist.linear.x;
+  const double target_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps;
+
+  const auto latest_published_cmd = m_ctrl_cmd_prev;  // use prev_cmd as a latest published command
+  if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) {
+    return false;  // not stopState: keep control
+  }
+
+  if (
+    std::fabs(current_vel) < m_stop_state_entry_ego_speed &&
+    std::fabs(target_vel) < m_stop_state_entry_target_speed) {
+    return true;
+  } else {
+    return false;
+  }
+}
+
+AckermannLateralCommand MpcLateralController::createCtrlCmdMsg(
+  const AckermannLateralCommand & ctrl_cmd)
+{
+  auto out = ctrl_cmd;
+  out.stamp = clock_->now();
+  m_steer_cmd_prev = out.steering_tire_angle;
+  return out;
+}
+
+void MpcLateralController::publishPredictedTraj(Trajectory & predicted_traj) const
+{
+  predicted_traj.header.stamp = clock_->now();
+  predicted_traj.header.frame_id = m_current_trajectory.header.frame_id;
+  m_pub_predicted_traj->publish(predicted_traj);
+}
+
+void MpcLateralController::publishDebugValues(Float32MultiArrayStamped & debug_values) const
+{
+  debug_values.stamp = clock_->now();
+  m_pub_debug_values->publish(debug_values);
+
+  Float32Stamped offset;
+  offset.stamp = clock_->now();
+  offset.data = steering_offset_->getOffset();
+  m_pub_steer_offset->publish(offset);
+}
+
+void MpcLateralController::setSteeringToHistory(const AckermannLateralCommand & steering)
+{
+  const auto time = clock_->now();
+  if (m_mpc_steering_history.empty()) {
+    m_mpc_steering_history.emplace_back(steering, time);
+    m_is_mpc_history_filled = false;
+    return;
+  }
+
+  m_mpc_steering_history.emplace_back(steering, time);
+
+  // Check the history is filled or not.
+  if (rclcpp::Duration(time - m_mpc_steering_history.begin()->second).seconds() >= 1.0) {
+    m_is_mpc_history_filled = true;
+    // remove old data that is older than 1 sec
+    for (auto itr = m_mpc_steering_history.begin(); itr != m_mpc_steering_history.end(); ++itr) {
+      if (rclcpp::Duration(time - itr->second).seconds() > 1.0) {
+        m_mpc_steering_history.erase(m_mpc_steering_history.begin());
+      } else {
+        break;
+      }
+    }
+  } else {
+    m_is_mpc_history_filled = false;
+  }
+}
+
+bool MpcLateralController::isMpcConverged()
+{
+  // If the number of variable below the 2, there is no enough data so MPC is not converged.
+  if (m_mpc_steering_history.size() < 2) {
+    return false;
+  }
+
+  // If the history is not filled, return false.
+
+  if (!m_is_mpc_history_filled) {
+    return false;
+  }
+
+  // Find the maximum and minimum values of the steering angle in the past 1 second.
+  double min_steering_value = m_mpc_steering_history[0].first.steering_tire_angle;
+  double max_steering_value = m_mpc_steering_history[0].first.steering_tire_angle;
+  for (size_t i = 1; i < m_mpc_steering_history.size(); i++) {
+    if (m_mpc_steering_history.at(i).first.steering_tire_angle < min_steering_value) {
+      min_steering_value = m_mpc_steering_history.at(i).first.steering_tire_angle;
+    }
+    if (m_mpc_steering_history.at(i).first.steering_tire_angle > max_steering_value) {
+      max_steering_value = m_mpc_steering_history.at(i).first.steering_tire_angle;
+    }
+  }
+  return (max_steering_value - min_steering_value) < m_mpc_converged_threshold_rps;
+}
+
+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");
+}
+
+rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+
+  // strong exception safety wrt MPCParam
+  MPCParam param = m_mpc.m_param;
+  auto & nw = param.nominal_weight;
+  auto & lcw = param.low_curvature_weight;
+
+  using MPCUtils::update_param;
+  try {
+    update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon);
+    update_param(parameters, "mpc_prediction_dt", param.prediction_dt);
+
+    const std::string ns_nw = "mpc_weight_";
+    update_param(parameters, ns_nw + "lat_error", nw.lat_error);
+    update_param(parameters, ns_nw + "heading_error", nw.heading_error);
+    update_param(parameters, ns_nw + "heading_error_squared_vel", nw.heading_error_squared_vel);
+    update_param(parameters, ns_nw + "steering_input", nw.steering_input);
+    update_param(parameters, ns_nw + "steering_input_squared_vel", nw.steering_input_squared_vel);
+    update_param(parameters, ns_nw + "lat_jerk", nw.lat_jerk);
+    update_param(parameters, ns_nw + "steer_rate", nw.steer_rate);
+    update_param(parameters, ns_nw + "steer_acc", nw.steer_acc);
+    update_param(parameters, ns_nw + "terminal_lat_error", nw.terminal_lat_error);
+    update_param(parameters, ns_nw + "terminal_heading_error", nw.terminal_heading_error);
+
+    const std::string ns_lcw = "mpc_low_curvature_weight_";
+    update_param(parameters, ns_lcw + "lat_error", lcw.lat_error);
+    update_param(parameters, ns_lcw + "heading_error", lcw.heading_error);
+    update_param(parameters, ns_lcw + "heading_error_squared_vel", lcw.heading_error_squared_vel);
+    update_param(parameters, ns_lcw + "steering_input", lcw.steering_input);
+    update_param(parameters, ns_lcw + "steering_input_squared_vel", lcw.steering_input_squared_vel);
+    update_param(parameters, ns_lcw + "lat_jerk", lcw.lat_jerk);
+    update_param(parameters, ns_lcw + "steer_rate", lcw.steer_rate);
+    update_param(parameters, ns_lcw + "steer_acc", lcw.steer_acc);
+
+    update_param(
+      parameters, "mpc_low_curvature_thresh_curvature", param.low_curvature_thresh_curvature);
+
+    update_param(parameters, "mpc_zero_ff_steer_deg", param.zero_ff_steer_deg);
+    update_param(parameters, "mpc_acceleration_limit", param.acceleration_limit);
+    update_param(parameters, "mpc_velocity_time_constant", param.velocity_time_constant);
+    update_param(parameters, "mpc_min_prediction_length", param.min_prediction_length);
+
+    // initialize input buffer
+    update_param(parameters, "input_delay", param.input_delay);
+    const double delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period);
+    const double delay = delay_step * m_mpc.m_ctrl_period;
+    if (param.input_delay != delay) {
+      param.input_delay = delay;
+      m_mpc.m_input_buffer = std::deque<double>(static_cast<size_t>(delay_step), 0.0);
+    }
+
+    // transaction succeeds, now assign values
+    m_mpc.m_param = param;
+  } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+    result.successful = false;
+    result.reason = e.what();
+  }
+
+  return result;
+}
+
+bool MpcLateralController::isTrajectoryShapeChanged() const
+{
+  // TODO(Horibe): update implementation to check trajectory shape around ego vehicle.
+  // Now temporally check the goal position.
+  for (const auto & trajectory : m_trajectory_buffer) {
+    const auto change_distance = tier4_autoware_utils::calcDistance2d(
+      trajectory.points.back().pose, m_current_trajectory.points.back().pose);
+    if (change_distance > m_new_traj_end_dist) {
+      return true;
+    }
+  }
+  return false;
+}
+
+bool MpcLateralController::isValidTrajectory(const Trajectory & traj) const
+{
+  for (const auto & p : traj.points) {
+    if (
+      !isfinite(p.pose.position.x) || !isfinite(p.pose.position.y) ||
+      !isfinite(p.pose.orientation.w) || !isfinite(p.pose.orientation.x) ||
+      !isfinite(p.pose.orientation.y) || !isfinite(p.pose.orientation.z) ||
+      !isfinite(p.longitudinal_velocity_mps) || !isfinite(p.lateral_velocity_mps) ||
+      !isfinite(p.lateral_velocity_mps) || !isfinite(p.heading_rate_rps) ||
+      !isfinite(p.front_wheel_angle_rad) || !isfinite(p.rear_wheel_angle_rad)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/mpc_lateral_controller/lowpass_filter.hpp

@@ -0,0 +1,105 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
+#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+/**
+ * @brief 2nd-order Butterworth Filter
+ * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930.
+ */
+class Butterworth2dFilter
+{
+private:
+  double m_y1;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double m_y2;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double m_u1;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double m_u2;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double m_a1;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double m_a2;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double m_b0;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double m_b1;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double m_b2;  //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+
+public:
+  /**
+   * @brief constructor with initialization
+   * @param [in] dt sampling time
+   * @param [in] f_cutoff_hz cutoff frequency [Hz]
+   */
+  explicit Butterworth2dFilter(double dt = 0.01, double f_cutoff_hz = 5.0);
+
+  /**
+   * @brief destructor
+   */
+  ~Butterworth2dFilter();
+
+  /**
+   * @brief constructor
+   * @param [in] dt sampling time
+   * @param [in] f_cutoff_hz cutoff frequency [Hz]
+   */
+  void initialize(const double & dt, const double & f_cutoff_hz);
+
+  /**
+   * @brief filtering (call this function at each sampling time with input)
+   * @param [in] u scalar input for filter
+   * @return filtered scalar value
+   */
+  double filter(const double & u);
+
+  /**
+   * @brief filtering for time-series data
+   * @param [in] t time-series data for input vector
+   * @param [out] u object vector
+   */
+  void filt_vector(const std::vector<double> & t, std::vector<double> & u) const;
+
+  /**
+   * @brief filtering for time-series data from both forward-backward direction for zero phase delay
+   * @param [in] t time-series data for input vector
+   * @param [out] u object vector
+   */
+  void filtfilt_vector(
+    const std::vector<double> & t,
+    std::vector<double> & u) const;  // filtering forward and backward direction
+
+  /**
+   * @brief get filter coefficients
+   * @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2].
+   */
+  void getCoefficients(std::vector<double> & coeffs) const;
+};
+
+/**
+ * @brief Move Average Filter
+ */
+namespace MoveAverageFilter
+{
+/**
+ * @brief filtering vector
+ * @param [in] num index distance for moving average filter
+ * @param [out] u object vector
+ */
+bool filt_vector(const int num, std::vector<double> & u);
+}  // namespace MoveAverageFilter
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_

src/decition/mpccontroller/mpc_lateral_controller/mpc.hpp

@@ -0,0 +1,525 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_
+#define MPC_LATERAL_CONTROLLER__MPC_HPP_
+
+#include "mpc_lateral_controller/lowpass_filter.hpp"
+#include "mpc_lateral_controller/mpc_trajectory.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
+#include "mpc_lateral_controller/steering_predictor.hpp"
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
+
+#include <deque>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+using autoware_auto_control_msgs::msg::AckermannLateralCommand;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_vehicle_msgs::msg::SteeringReport;
+using geometry_msgs::msg::Pose;
+using nav_msgs::msg::Odometry;
+using tier4_debug_msgs::msg::Float32MultiArrayStamped;
+
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+
+// Weight factors used in Model Predictive Control
+struct MPCWeight
+{
+  // Weight for lateral tracking error. A larger weight leads to less lateral tracking error.
+  double lat_error;
+
+  // Weight for heading tracking error. A larger weight reduces heading tracking error.
+  double heading_error;
+
+  // Weight combining heading error and velocity. Adjusts the influence of heading error based on
+  // velocity.
+  double heading_error_squared_vel;
+
+  // Weight for lateral tracking error at the terminal state of the trajectory. This improves the
+  // stability of MPC.
+  double terminal_lat_error;
+
+  // Weight for heading tracking error at the terminal state of the trajectory. This improves the
+  // stability of MPC.
+  double terminal_heading_error;
+
+  // Weight for the steering input. This surpress the deviation between the steering command and
+  // reference steering angle calculated from curvature.
+  double steering_input;
+
+  // Adjusts the influence of steering_input weight based on velocity.
+  double steering_input_squared_vel;
+
+  // Weight for lateral jerk. Penalizes sudden changes in lateral acceleration.
+  double lat_jerk;
+
+  // Weight for steering rate. Penalizes the speed of steering angle change.
+  double steer_rate;
+
+  // Weight for steering angle acceleration. Regulates the rate of change of steering rate.
+  double steer_acc;
+};
+
+struct MPCParam
+{
+  // Number of steps in the prediction horizon.
+  int prediction_horizon;
+
+  // Sampling time for the prediction horizon.
+  double prediction_dt;
+
+  // Threshold at which the feed-forward steering angle becomes zero.
+  double zero_ff_steer_deg;
+
+  // Time delay for compensating the steering input.
+  double input_delay;
+
+  // Limit for calculating trajectory velocity.
+  double acceleration_limit;
+
+  // Time constant for calculating trajectory velocity.
+  double velocity_time_constant;
+
+  // Minimum prediction distance used for low velocity case.
+  double min_prediction_length;
+
+  // Time constant for the steer model.
+  double steer_tau;
+
+  // Weight parameters for the MPC in nominal conditions.
+  MPCWeight nominal_weight;
+
+  // Weight parameters for the MPC in low curvature path conditions.
+  MPCWeight low_curvature_weight;
+
+  // Curvature threshold to determine when to use "low curvature" parameter settings.
+  double low_curvature_thresh_curvature;
+};
+
+struct TrajectoryFilteringParam
+{
+  // path resampling interval [m]
+  double traj_resample_dist;
+
+  // flag of traj extending for terminal yaw
+  bool extend_trajectory_for_end_yaw_control;
+
+  // flag for path smoothing
+  bool enable_path_smoothing;
+
+  // param of moving average filter for path smoothing
+  int path_filter_moving_ave_num;
+
+  // point-to-point index distance for curvature calculation for trajectory
+  int curvature_smoothing_num_traj;
+
+  // point-to-point index distance for curvature calculation for reference steer command
+  int curvature_smoothing_num_ref_steer;
+};
+
+struct MPCData
+{
+  // Index of the nearest point in the trajectory.
+  size_t nearest_idx{};
+
+  // Time stamp of the nearest point in the trajectory.
+  double nearest_time{};
+
+  // Pose (position and orientation) of the nearest point in the trajectory.
+  Pose nearest_pose{};
+
+  // Current steering angle.
+  double steer{};
+
+  // Predicted steering angle based on the vehicle model.
+  double predicted_steer{};
+
+  // Lateral tracking error.
+  double lateral_err{};
+
+  // Yaw (heading) tracking error.
+  double yaw_err{};
+
+  MPCData() = default;
+};
+
+/**
+ * MPC matrix with the following format:
+ * Xex = Aex * X0 + Bex * Uex * Wex
+ * Yex = Cex * Xex
+ * Cost = Xex' * Qex * Xex + (Uex - Uref_ex)' * R1ex * (Uex - Uref_ex) +  Uex' * R2ex * Uex
+ */
+struct MPCMatrix
+{
+  MatrixXd Aex;
+  MatrixXd Bex;
+  MatrixXd Wex;
+  MatrixXd Cex;
+  MatrixXd Qex;
+  MatrixXd R1ex;
+  MatrixXd R2ex;
+  MatrixXd Uref_ex;
+
+  MPCMatrix() = default;
+};
+
+/**
+ * MPC-based waypoints follower class
+ * @brief calculate control command to follow reference waypoints
+ */
+class MPC
+{
+private:
+  rclcpp::Logger m_logger = rclcpp::get_logger("mpc_logger");  // ROS logger used for debug logging.
+  rclcpp::Clock::SharedPtr m_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);  // ROS clock.
+
+  // Vehicle model used for MPC.
+  std::shared_ptr<VehicleModelInterface> m_vehicle_model_ptr;
+
+  // QP solver used for MPC.
+  std::shared_ptr<QPSolverInterface> m_qpsolver_ptr;
+
+  // Calculate predicted steering angle based on the steering dynamics. The predicted value should
+  // fit to the actual steering angle if the vehicle model is accurate enough.
+  std::shared_ptr<SteeringPredictor> m_steering_predictor;
+
+  Butterworth2dFilter m_lpf_steering_cmd;   // Low-pass filter for smoothing the steering command.
+  Butterworth2dFilter m_lpf_lateral_error;  // Low-pass filter for smoothing the lateral error.
+  Butterworth2dFilter m_lpf_yaw_error;      // Low-pass filter for smoothing the heading error.
+
+  double m_raw_steer_cmd_pprev = 0.0;  // Raw output computed two iterations ago.
+  double m_lateral_error_prev = 0.0;   // Previous lateral error for derivative calculation.
+  double m_yaw_error_prev = 0.0;       // Previous heading error for derivative calculation.
+
+  bool m_is_forward_shift = true;  // Flag indicating if the shift is in the forward direction.
+
+  double m_min_prediction_length = 5.0;  // Minimum prediction distance.
+
+  /**
+   * @brief Get variables for MPC calculation.
+   * @param trajectory The reference trajectory.
+   * @param current_steer The current steering report.
+   * @param current_kinematics The current vehicle kinematics.
+   * @return A pair of a boolean flag indicating success and the MPC data.
+   */
+  std::pair<bool, MPCData> getData(
+    const MPCTrajectory & trajectory, const SteeringReport & current_steer,
+    const Odometry & current_kinematics);
+
+  /**
+   * @brief Get the initial state for MPC.
+   * @param data The MPC data.
+   * @return The initial state as a vector.
+   */
+  VectorXd getInitialState(const MPCData & data);
+
+  /**
+   * @brief Update the state for delay compensation.
+   * @param traj The reference trajectory to follow.
+   * @param start_time The time where x0_orig is defined.
+   * @param x0_orig The original initial state vector.
+   * @return A pair of a boolean flag indicating success and the updated state at delayed_time.
+   */
+  std::pair<bool, VectorXd> updateStateForDelayCompensation(
+    const MPCTrajectory & traj, const double & start_time, const VectorXd & x0_orig);
+
+  /**
+   * @brief Generate the MPC matrix using the reference trajectory and vehicle model.
+   * @param reference_trajectory The reference trajectory used for linearization.
+   * @param prediction_dt The prediction time step.
+   * @return The generated MPC matrix.
+   */
+  MPCMatrix generateMPCMatrix(
+    const MPCTrajectory & reference_trajectory, const double prediction_dt);
+
+  /**
+   * @brief Execute the optimization using the provided MPC matrix, initial state, and prediction
+   * time step.
+   * @param mpc_matrix The parameters matrix used for optimization.
+   * @param x0 The initial state vector.
+   * @param prediction_dt The prediction time step.
+   * @param [in] trajectory mpc reference trajectory
+   * @param [in] current_velocity current ego velocity
+   * @return A pair of a boolean flag indicating success and the optimized input vector.
+   */
+  std::pair<bool, VectorXd> executeOptimization(
+    const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt,
+    const MPCTrajectory & trajectory, const double current_velocity);
+
+  /**
+   * @brief Resample the trajectory with the MPC resampling time.
+   * @param start_time The start time for resampling.
+   * @param prediction_dt The prediction time step.
+   * @param input The input trajectory.
+   * @return A pair of a boolean flag indicating success and the resampled trajectory.
+   */
+  std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByTime(
+    const double start_time, const double prediction_dt, const MPCTrajectory & input) const;
+
+  /**
+   * @brief Apply the velocity dynamics filter to the trajectory using the current kinematics.
+   * @param trajectory The input trajectory.
+   * @param current_kinematics The current vehicle kinematics.
+   * @return The filtered trajectory.
+   */
+  MPCTrajectory applyVelocityDynamicsFilter(
+    const MPCTrajectory & trajectory, const Odometry & current_kinematics) const;
+
+  /**
+   * @brief Get the prediction time step for MPC. If the trajectory length is shorter than
+   * min_prediction_length, adjust the time step.
+   * @param start_time The start time of the trajectory.
+   * @param input The input trajectory.
+   * @param current_kinematics The current vehicle kinematics.
+   * @return The prediction time step.
+   */
+  double getPredictionDeltaTime(
+    const double start_time, const MPCTrajectory & input,
+    const Odometry & current_kinematics) const;
+
+  /**
+   * @brief Add weights related to lateral jerk, steering rate, and steering acceleration to the R
+   * matrix.
+   * @param prediction_dt The prediction time step.
+   * @param R The R matrix to modify.
+   */
+  void addSteerWeightR(const double prediction_dt, MatrixXd & R) const;
+
+  /**
+   * @brief Add weights related to lateral jerk, steering rate, and steering acceleration to the f
+   * matrix.
+   * @param prediction_dt The prediction time step.
+   * @param f The f matrix to modify.
+   */
+  void addSteerWeightF(const double prediction_dt, MatrixXd & f) const;
+
+  /**
+   * @brief Calculate the desired steering rate for the steering_rate command.
+   * @param m The MPC matrix used for optimization.
+   * @param x0 The initial state matrix.
+   * @param Uex The input matrix.
+   * @param u_filtered The filtered input.
+   * @param current_steer The current steering angle.
+   * @param predict_dt The prediction time step.
+   * @return The desired steering rate.
+   */
+  double calcDesiredSteeringRate(
+    const MPCMatrix & m, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered,
+    const float current_steer, const double predict_dt) const;
+
+  /**
+   * @brief Check if the MPC matrix has any invalid values.
+   * @param m The MPC matrix to check.
+   * @return True if the matrix is valid, false otherwise.
+   */
+  bool isValid(const MPCMatrix & m) const;
+
+  /**
+   * @brief Get the weight for the MPC optimization based on the curvature.
+   * @param curvature The curvature value.
+   * @return The weight for the MPC optimization.
+   */
+  inline MPCWeight getWeight(const double curvature)
+  {
+    return std::fabs(curvature) < m_param.low_curvature_thresh_curvature
+             ? m_param.low_curvature_weight
+             : m_param.nominal_weight;
+  }
+
+  /**
+   * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result.
+   * @param mpc_resampled_reference_trajectory The resampled reference trajectory.
+   * @param mpc_matrix The MPC matrix used for optimization.
+   * @param x0_delayed The delayed initial state vector.
+   * @param Uex The optimized input vector.
+   * @return The predicted trajectory.
+   */
+  Trajectory calcPredictedTrajectory(
+    const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix,
+    const VectorXd & x0_delayed, const VectorXd & Uex) const;
+
+  /**
+   * @brief Generate diagnostic data for debugging purposes.
+   * @param reference_trajectory The reference trajectory.
+   * @param mpc_data The MPC data.
+   * @param mpc_matrix The MPC matrix.
+   * @param ctrl_cmd The control command.
+   * @param Uex The optimized input vector.
+   * @param current_kinematics The current vehicle kinematics.
+   * @return The generated diagnostic data.
+   */
+  Float32MultiArrayStamped generateDiagData(
+    const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
+    const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
+    const Odometry & current_kinematics) const;
+
+  /**
+   * @brief calculate steering rate limit along with the target trajectory
+   * @param reference_trajectory The reference trajectory.
+   * @param current_velocity current velocity of ego.
+   */
+  VectorXd calcSteerRateLimitOnTrajectory(
+    const MPCTrajectory & trajectory, const double current_velocity) const;
+
+  //!< @brief logging with warn and return false
+  template <typename... Args>
+  inline bool fail_warn_throttle(Args &&... args) const
+  {
+    RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...);
+    return false;
+  }
+
+  //!< @brief logging with warn
+  template <typename... Args>
+  inline void warn_throttle(Args &&... args) const
+  {
+    RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...);
+  }
+
+public:
+  MPCTrajectory m_reference_trajectory;  // Reference trajectory to be followed.
+  MPCParam m_param;                      // MPC design parameters.
+  std::deque<double> m_input_buffer;     // MPC output buffer for delay time compensation.
+  double m_raw_steer_cmd_prev = 0.0;     // Previous MPC raw output.
+
+  /* Parameters for control */
+  double m_admissible_position_error;  // Threshold for lateral error to trigger stop command [m].
+  double m_admissible_yaw_error_rad;   // Threshold for yaw error to trigger stop command [rad].
+  double m_steer_lim;                  // Steering command limit [rad].
+  double m_ctrl_period;                // Control frequency [s].
+
+  //!< @brief steering rate limit list depending on curvature [/m], [rad/s]
+  std::vector<std::pair<double, double>> m_steer_rate_lim_map_by_curvature{};
+
+  //!< @brief steering rate limit list depending on velocity [m/s], [rad/s]
+  std::vector<std::pair<double, double>> m_steer_rate_lim_map_by_velocity{};
+
+  bool m_use_steer_prediction;  // Flag to use predicted steer instead of measured steer.
+  double ego_nearest_dist_threshold = 3.0;  // Threshold for nearest index search based on distance.
+  double ego_nearest_yaw_threshold = M_PI_2;  // Threshold for nearest index search based on yaw.
+
+  //!< Constructor.
+  MPC() = default;
+
+  /**
+   * @brief Calculate control command using the MPC algorithm.
+   * @param current_steer Current steering report.
+   * @param current_kinematics Current vehicle kinematics.
+   * @param ctrl_cmd Computed lateral control command.
+   * @param predicted_trajectory Predicted trajectory based on MPC result.
+   * @param diagnostic Diagnostic data for debugging purposes.
+   * @return True if the MPC calculation is successful, false otherwise.
+   */
+  bool calculateMPC(
+    const SteeringReport & current_steer, const Odometry & current_kinematics,
+    AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
+    Float32MultiArrayStamped & diagnostic);
+
+  /**
+   * @brief Set the reference trajectory to be followed.
+   * @param trajectory_msg The reference trajectory message.
+   * @param param Trajectory filtering parameters.
+   */
+  void setReferenceTrajectory(
+    const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
+    const Odometry & current_kinematics);
+
+  /**
+   * @brief Reset the previous result of MPC.
+   * @param current_steer Current steering report.
+   */
+  void resetPrevResult(const SteeringReport & current_steer);
+
+  /**
+   * @brief Set the vehicle model for this MPC.
+   * @param vehicle_model_ptr Pointer to the vehicle model.
+   */
+  inline void setVehicleModel(std::shared_ptr<VehicleModelInterface> vehicle_model_ptr)
+  {
+    m_vehicle_model_ptr = vehicle_model_ptr;
+  }
+
+  /**
+   * @brief Set the QP solver for this MPC.
+   * @param qpsolver_ptr Pointer to the QP solver.
+   */
+  inline void setQPSolver(std::shared_ptr<QPSolverInterface> qpsolver_ptr)
+  {
+    m_qpsolver_ptr = qpsolver_ptr;
+  }
+
+  /**
+   * @brief Initialize the steering predictor for this MPC.
+   */
+  inline void initializeSteeringPredictor()
+  {
+    m_steering_predictor =
+      std::make_shared<SteeringPredictor>(m_param.steer_tau, m_param.input_delay);
+  }
+
+  /**
+   * @brief Initialize the low-pass filters.
+   * @param steering_lpf_cutoff_hz Cutoff frequency for the steering command low-pass filter.
+   * @param error_deriv_lpf_cutoff_hz Cutoff frequency for the error derivative low-pass filter.
+   */
+  inline void initializeLowPassFilters(
+    const double steering_lpf_cutoff_hz, const double error_deriv_lpf_cutoff_hz)
+  {
+    m_lpf_steering_cmd.initialize(m_ctrl_period, steering_lpf_cutoff_hz);
+    m_lpf_lateral_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz);
+    m_lpf_yaw_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz);
+  }
+
+  /**
+   * @brief Check if the MPC has a vehicle model set.
+   * @return True if the vehicle model is set, false otherwise.
+   */
+  inline bool hasVehicleModel() const { return m_vehicle_model_ptr != nullptr; }
+
+  /**
+   * @brief Check if the MPC has a QP solver set.
+   * @return True if the QP solver is set, false otherwise.
+   */
+  inline bool hasQPSolver() const { return m_qpsolver_ptr != nullptr; }
+
+  /**
+   * @brief Set the RCLCPP logger to be used for logging.
+   * @param logger The RCLCPP logger object.
+   */
+  inline void setLogger(rclcpp::Logger logger) { m_logger = logger; }
+
+  /**
+   * @brief Set the RCLCPP clock to be used for time keeping.
+   * @param clock The shared pointer to the RCLCPP clock.
+   */
+  inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; }
+};  // class MPC
+}  // namespace autoware::motion::control::mpc_lateral_controller
+
+#endif  // MPC_LATERAL_CONTROLLER__MPC_HPP_

src/decition/mpccontroller/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -0,0 +1,285 @@
+// Copyright 2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_
+#define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_
+
+#include "mpc_lateral_controller/mpc.hpp"
+#include "mpc_lateral_controller/mpc_trajectory.hpp"
+#include "mpc_lateral_controller/mpc_utils.hpp"
+#include "mpc_lateral_controller/steering_offset/steering_offset.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "trajectory_follower_base/lateral_controller_base.hpp"
+
+#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
+#include "tier4_debug_msgs/msg/float32_stamped.hpp"
+
+#include <deque>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
+using autoware_auto_control_msgs::msg::AckermannLateralCommand;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_vehicle_msgs::msg::SteeringReport;
+using nav_msgs::msg::Odometry;
+using tier4_debug_msgs::msg::Float32MultiArrayStamped;
+using tier4_debug_msgs::msg::Float32Stamped;
+
+class MpcLateralController : public trajectory_follower::LateralControllerBase
+{
+public:
+  /// \param node Reference to the node used only for the component and parameter initialization.
+  explicit MpcLateralController(rclcpp::Node & node);
+  virtual ~MpcLateralController();
+
+private:
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Logger logger_;
+
+  rclcpp::Publisher<Trajectory>::SharedPtr m_pub_predicted_traj;
+  rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr m_pub_debug_values;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr m_pub_steer_offset;
+
+  //!< @brief parameters for path smoothing
+  TrajectoryFilteringParam m_trajectory_filtering_param;
+
+  // Ego vehicle speed threshold to enter the stop state.
+  double m_stop_state_entry_ego_speed;
+
+  // Target vehicle speed threshold to enter the stop state.
+  double m_stop_state_entry_target_speed;
+
+  // Convergence threshold for steering control.
+  double m_converged_steer_rad;
+
+  // max mpc output change threshold for 1 sec
+  double m_mpc_converged_threshold_rps;
+
+  // Time duration threshold to check if the trajectory shape has changed.
+  double m_new_traj_duration_time;
+
+  // Distance threshold to check if the trajectory shape has changed.
+  double m_new_traj_end_dist;
+
+  // Flag indicating whether to keep the steering control until it converges.
+  bool m_keep_steer_control_until_converged;
+
+  // trajectory buffer for detecting new trajectory
+  std::deque<Trajectory> m_trajectory_buffer;
+
+  MPC m_mpc;  // MPC object for trajectory following.
+
+  // Check is mpc output converged
+  bool m_is_mpc_history_filled{false};
+
+  // store the last mpc outputs for 1 sec
+  std::vector<std::pair<AckermannLateralCommand, rclcpp::Time>> m_mpc_steering_history{};
+
+  // set the mpc steering output to history
+  void setSteeringToHistory(const AckermannLateralCommand & steering);
+
+  // check if the mpc steering output is converged
+  bool isMpcConverged();
+
+  // measured kinematic state
+  Odometry m_current_kinematic_state;
+
+  SteeringReport m_current_steering;  // Measured steering information.
+
+  Trajectory m_current_trajectory;  // Current reference trajectory for path following.
+
+  double m_steer_cmd_prev = 0.0;  // MPC output in the previous period.
+
+  // Flag indicating whether the previous control command is initialized.
+  bool m_is_ctrl_cmd_prev_initialized = false;
+
+  // Previous control command for path following.
+  AckermannLateralCommand m_ctrl_cmd_prev;
+
+  //  Flag indicating whether the first trajectory has been received.
+  bool m_has_received_first_trajectory = false;
+
+  // Threshold distance for the ego vehicle in nearest index search.
+  double m_ego_nearest_dist_threshold;
+
+  // Threshold yaw for the ego vehicle in nearest index search.
+  double m_ego_nearest_yaw_threshold;
+
+  // Flag indicating whether auto steering offset removal is enabled.
+  bool enable_auto_steering_offset_removal_;
+
+  // Steering offset estimator for offset compensation.
+  std::shared_ptr<SteeringOffsetEstimator> steering_offset_;
+
+  /**
+   * @brief Initialize the timer
+   * @param period_s Control period in seconds.
+   */
+  void initTimer(double period_s);
+
+  /**
+   * @brief Create the vehicle model based on the provided parameters.
+   * @param wheelbase Vehicle's wheelbase.
+   * @param steer_lim Steering command limit.
+   * @param steer_tau Steering time constant.
+   * @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);
+
+  /**
+   * @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);
+
+  /**
+   * @brief Create the steering offset estimator for offset compensation.
+   * @param wheelbase Vehicle's wheelbase.
+   * @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);
+
+  /**
+   * @brief Check if all necessary data is received and ready to run the control.
+   * @param input_data Input data required for control calculation.
+   * @return True if the data is ready, false otherwise.
+   */
+  bool isReady(const trajectory_follower::InputData & input_data) override;
+
+  /**
+   * @brief Compute the control command for path following with a constant control period.
+   * @param input_data Input data required for control calculation.
+   * @return Lateral output control command.
+   */
+  trajectory_follower::LateralOutput run(
+    trajectory_follower::InputData const & input_data) override;
+
+  /**
+   * @brief Set the current trajectory using the received message.
+   * @param msg Received trajectory message.
+   */
+  void setTrajectory(const Trajectory & msg, const Odometry & current_kinematics);
+
+  /**
+   * @brief Check if the received data is valid.
+   * @return True if the data is valid, false otherwise.
+   */
+  bool checkData() const;
+
+  /**
+   * @brief Create the control command.
+   * @param ctrl_cmd Control command to be created.
+   * @return Created control command.
+   */
+  AckermannLateralCommand createCtrlCmdMsg(const AckermannLateralCommand & ctrl_cmd);
+
+  /**
+   * @brief Publish the predicted future trajectory.
+   * @param predicted_traj Predicted future trajectory to be published.
+   */
+  void publishPredictedTraj(Trajectory & predicted_traj) const;
+
+  /**
+   * @brief Publish diagnostic message.
+   * @param diagnostic Diagnostic message to be published.
+   */
+  void publishDebugValues(Float32MultiArrayStamped & diagnostic) const;
+
+  /**
+   * @brief Get the stop control command.
+   * @return Stop control command.
+   */
+  AckermannLateralCommand getStopControlCommand() const;
+
+  /**
+   * @brief Get the control command applied before initialization.
+   * @return Initial control command.
+   */
+  AckermannLateralCommand getInitialControlCommand() const;
+
+  /**
+   * @brief Check if the ego car is in a stopped state.
+   * @return True if the ego car is stopped, false otherwise.
+   */
+  bool isStoppedState() const;
+
+  /**
+   * @brief Check if the trajectory has a valid value.
+   * @param traj Trajectory to be checked.
+   * @return True if the trajectory is valid, false otherwise.
+   */
+  bool isValidTrajectory(const Trajectory & traj) const;
+
+  /**
+   * @brief Check if the trajectory shape has changed.
+   * @return True if the trajectory shape has changed, false otherwise.
+   */
+  bool isTrajectoryShapeChanged() const;
+
+  /**
+   * @brief Check if the steering control is converged and stable now.
+   * @param cmd Steering control command to be checked.
+   * @return True if the steering control is converged and stable, false otherwise.
+   */
+  bool isSteerConverged(const AckermannLateralCommand & cmd) const;
+
+  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
+
+  /**
+   * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly.
+   * @param node Reference to the node.
+   */
+  void declareMPCparameters(rclcpp::Node & node);
+
+  /**
+   * @brief Callback function called when parameters are changed outside of the node.
+   * @param parameters Vector of changed parameters.
+   * @return Result of the parameter callback.
+   */
+  rcl_interfaces::msg::SetParametersResult paramCallback(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  template <typename... Args>
+  inline void info_throttle(Args &&... args)
+  {
+    RCLCPP_INFO_THROTTLE(logger_, *clock_, 5000, args...);
+  }
+
+  template <typename... Args>
+  inline void warn_throttle(Args &&... args)
+  {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, args...);
+  }
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+
+#endif  // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_

src/decition/mpccontroller/mpc_lateral_controller/mpc_trajectory.hpp

@@ -0,0 +1,122 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_
+#define MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_
+
+//#include "tier4_autoware_utils/geometry/geometry.hpp"
+
+//#include "geometry_msgs/msg/point.hpp"
+
+#include <iostream>
+#include <vector>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/**
+ * Trajectory class for mpc follower
+ * @brief calculate control command to follow reference waypoints
+ */
+
+class MPCTrajectoryPoint
+{
+public:
+  double x;
+  double y;
+  double z;
+  double yaw;
+  double vx;
+  double k;
+  double smooth_k;
+  double relative_time;
+};
+
+// This class individually maintains an array of each element. This allows for easy application of
+// filtering processes. For example: filter_vector(mpc_trajectory.x).
+class MPCTrajectory
+{
+public:
+  std::vector<double> x;              //!< @brief x position x vector
+  std::vector<double> y;              //!< @brief y position y vector
+  std::vector<double> z;              //!< @brief z position z vector
+  std::vector<double> yaw;            //!< @brief yaw pose yaw vector
+  std::vector<double> vx;             //!< @brief vx velocity vx vector
+  std::vector<double> k;              //!< @brief k curvature k vector
+  std::vector<double> smooth_k;       //!< @brief k smoothed-curvature k vector
+  std::vector<double> relative_time;  //!< @brief relative_time duration time from start point
+
+  /**
+   * @brief push_back for all values
+   */
+  void push_back(
+    const double & xp, const double & yp, const double & zp, const double & yawp,
+    const double & vxp, const double & kp, const double & smooth_kp, const double & tp);
+
+  /**
+   * @brief push_back for all values
+   */
+  void push_back(const MPCTrajectoryPoint & p);
+
+  /**
+   * @brief Get the last element. Apply back() for all vectors.
+   */
+  MPCTrajectoryPoint back();
+
+  /**
+   * @brief clear for all values
+   */
+  void clear();
+
+  /**
+   * @brief check size of MPCTrajectory
+   * @return size, or 0 if the size for each components are inconsistent
+   */
+  size_t size() const;
+
+  /**
+   * @return true if the components sizes are all 0 or are inconsistent
+   */
+  inline bool empty() const { return size() == 0; }
+
+//  std::vector<geometry_msgs::msg::Point> toPoints() const
+//  {
+//    std::vector<geometry_msgs::msg::Point> points;
+//    for (size_t i = 0; i < x.size(); ++i) {
+//      geometry_msgs::msg::Point point;
+//      point.x = x.at(i);
+//      point.y = y.at(i);
+//      point.z = z.at(i);
+//      points.push_back(point);
+//    }
+//    return points;
+//  }
+
+//  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> toTrajectoryPoints() const
+//  {
+//    std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> points;
+//    for (size_t i = 0; i < x.size(); ++i) {
+//      autoware_auto_planning_msgs::msg::TrajectoryPoint point;
+//      point.pose.position.x = x.at(i);
+//      point.pose.position.y = y.at(i);
+//      point.pose.position.z = z.at(i);
+//      point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw.at(i));
+//      point.longitudinal_velocity_mps = vx.at(i);
+//      points.push_back(point);
+//    }
+//    return points;
+//  }
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_

src/decition/mpccontroller/mpc_lateral_controller/mpc_utils.hpp

@@ -0,0 +1,218 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
+#define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "tf2/utils.h"
+
+#include <Eigen/Core>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#else
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#endif
+
+#include "mpc_lateral_controller/mpc_trajectory.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+
+#include <cmath>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+namespace MPCUtils
+{
+
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using geometry_msgs::msg::Pose;
+
+/**
+ * @brief calculate 2d distance from trajectory[idx1] to trajectory[idx2]
+ */
+double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2);
+
+/**
+ * @brief calculate 3d distance from trajectory[idx1] to trajectory[idx2]
+ */
+double calcDistance3d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2);
+
+/**
+ * @brief convert Euler angle vector including +-2pi to 0 jump to continuous series data
+ * @param [inout] angle_vector input angle vector
+ */
+void convertEulerAngleToMonotonic(std::vector<double> & angle_vector);
+
+/**
+ * @brief calculate the lateral error of the given pose relative to the given reference pose
+ * @param [in] ego_pose pose to check for error
+ * @param [in] ref_pose reference pose
+ * @return lateral distance between the two poses
+ */
+double calcLateralError(const Pose & ego_pose, const Pose & ref_pose);
+
+/**
+ * @brief convert the given Trajectory msg to a MPCTrajectory object
+ * @param [in] input trajectory to convert
+ * @return resulting MPCTrajectory
+ */
+MPCTrajectory convertToMPCTrajectory(const Trajectory & input);
+
+/**
+ * @brief convert the given MPCTrajectory to a Trajectory msg
+ * @param [in] input MPCTrajectory to be converted
+ * @return output converted Trajectory msg
+ */
+Trajectory convertToAutowareTrajectory(const MPCTrajectory & input);
+
+/**
+ * @brief calculate the arc length at each point of the given trajectory
+ * @param [in] trajectory trajectory for which to calculate the arc length
+ * @param [out] arc_length the cumulative arc length at each point of the trajectory
+ */
+void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<double> & arc_length);
+
+/**
+ * @brief resample the given trajectory with the given fixed interval
+ * @param [in] input trajectory to resample
+ * @param [in] resample_interval_dist the desired distance between two successive trajectory points
+ * @return The pair contains the successful flag and the resultant resampled trajectory
+ */
+std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
+  const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx,
+  const double ego_offset_to_segment);
+
+/**
+ * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired
+ * indexing
+ * @param [in] in_index indexes for each trajectory point
+ * @param [in] in_traj MPCTrajectory to interpolate
+ * @param [in] out_index desired interpolated indexes
+ * @param [out] out_traj resulting interpolated MPCTrajectory
+ */
+bool linearInterpMPCTrajectory(
+  const std::vector<double> & in_index, const MPCTrajectory & in_traj,
+  const std::vector<double> & out_index, MPCTrajectory & out_traj);
+
+/**
+ * @brief fill the relative_time field of the given MPCTrajectory
+ * @param [in] traj MPCTrajectory for which to fill in the relative_time
+ * @return true if the calculation was successful
+ */
+bool calcMPCTrajectoryTime(MPCTrajectory & traj);
+
+/**
+ * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing
+ * @param [in] start_seg_idx segment index of the trajectory point from which to start smoothing
+ * @param [in] start_vel initial velocity to set at the start_seg_idx
+ * @param [in] acc_lim limit on the acceleration
+ * @param [in] tau constant to control the smoothing (high-value = very smooth)
+ * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity
+ */
+void dynamicSmoothingVelocity(
+  const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
+  MPCTrajectory & traj);
+
+/**
+ * @brief calculate yaw angle in MPCTrajectory from xy vector
+ * @param [inout] traj object trajectory
+ * @param [in] shift is forward or not
+ */
+void calcTrajectoryYawFromXY(MPCTrajectory & traj, const bool is_forward_shift);
+
+/**
+ * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3
+ * points when num = 1)
+ * @param [in] curvature_smoothing_num_traj index distance for 3 points for curvature calculation
+ * @param [in] curvature_smoothing_num_ref_steer index distance for 3 points for smoothed curvature
+ * calculation
+ * @param [inout] traj object trajectory
+ */
+void calcTrajectoryCurvature(
+  const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer,
+  MPCTrajectory & traj);
+
+/**
+ * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3
+ * points when num = 1)
+ * @param [in] curvature_smoothing_num index distance for 3 points for curvature calculation
+ * @param [in] traj input trajectory
+ * @return vector of curvatures at each point of the given trajectory
+ */
+std::vector<double> calcTrajectoryCurvature(
+  const int curvature_smoothing_num, const MPCTrajectory & traj);
+
+/**
+ * @brief calculate nearest pose on MPCTrajectory with linear interpolation
+ * @param [in] traj reference trajectory
+ * @param [in] self_pose object pose
+ * @param [out] nearest_pose nearest pose on path
+ * @param [out] nearest_index path index of nearest pose
+ * @param [out] nearest_time time of nearest pose on trajectory
+ * @return false when nearest pose couldn't find for some reasons
+ */
+bool calcNearestPoseInterp(
+  const MPCTrajectory & traj, const Pose & self_pose, Pose * nearest_pose, size_t * nearest_index,
+  double * nearest_time, const double max_dist, const double max_yaw);
+
+/**
+ * @brief calculate distance to stopped point
+ */
+double calcStopDistance(const Trajectory & current_trajectory, const int origin);
+
+/**
+ * @brief extend terminal points
+ * Note: The current MPC does not properly take into account the attitude angle at the end of the
+ * path. By extending the end of the path in the attitude direction, the MPC can consider the
+ * attitude angle well, resulting in improved control performance. If the trajectory is
+ * well-defined considering the end point attitude angle, this feature is not necessary.
+ * @param [in] terminal yaw
+ * @param [in] extend interval
+ * @param [in] flag of forward shift
+ * @param [out] extended trajectory
+ */
+void extendTrajectoryInYawDirection(
+  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj);
+
+/**
+ * @brief Updates the value of a parameter with the given name.
+ * @tparam T The type of the parameter value.
+ * @param parameters A vector of rclcpp::Parameter objects.
+ * @param name The name of the parameter to update.
+ * @param value A reference variable to store the updated parameter value.
+ */
+template <typename T>
+void update_param(
+  const std::vector<rclcpp::Parameter> & parameters, const std::string & name, T & value)
+{
+  auto it = std::find_if(
+    parameters.cbegin(), parameters.cend(),
+    [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; });
+  if (it != parameters.cend()) {
+    value = static_cast<T>(it->template get_value<T>());
+  }
+}
+
+}  // namespace MPCUtils
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_

src/decition/mpccontroller/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp

@@ -0,0 +1,50 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_
+#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_
+
+#include <Eigen/Core>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/// Interface for solvers of Quadratic Programming (QP) problems
+class QPSolverInterface
+{
+public:
+  /**
+   * @brief destructor
+   */
+  virtual ~QPSolverInterface() = default;
+
+  /**
+   * @brief solve QP problem : minimize J = u' * h_mat * u + f_vec' * u without constraint
+   * @param [in] h_mat parameter matrix in object function
+   * @param [in] f_vec parameter matrix in object function
+   * @param [in] a parameter matrix for constraint lb_a < a*u < ub_a
+   * @param [in] lb parameter matrix for constraint lb < u < ub
+   * @param [in] ub parameter matrix for constraint lb < u < ub
+   * @param [in] lb_a parameter matrix for constraint lb_a < a*u < ub_a
+   * @param [in] ub_a parameter matrix for constraint lb_a < a*u < ub_a
+   * @param [out] u optimal variable vector
+   * @return true if the problem was solved
+   */
+  virtual bool solve(
+    const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
+    const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
+    const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0;
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_

src/decition/mpccontroller/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp

@@ -0,0 +1,62 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
+#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
+
+#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
+#include "osqp_interface/osqp_interface.hpp"
+//#include "rclcpp/rclcpp.hpp"
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/// Solver for QP problems using the OSQP library
+class QPSolverOSQP : public QPSolverInterface
+{
+public:
+  /**
+   * @brief constructor
+   */
+//  explicit QPSolverOSQP(const rclcpp::Logger & logger);
+  explicit QPSolverOSQP();
+
+  /**
+   * @brief destructor
+   */
+  virtual ~QPSolverOSQP() = default;
+
+  /**
+   * @brief solve QP problem : minimize j = u' * h_mat * u + f_vec' * u without constraint
+   * @param [in] h_mat parameter matrix in object function
+   * @param [in] f_vec parameter matrix in object function
+   * @param [in] a parameter matrix for constraint lb_a < a*u < ub_a (not used here)
+   * @param [in] lb parameter matrix for constraint lb < U < ub (not used here)
+   * @param [in] ub parameter matrix for constraint lb < U < ub (not used here)
+   * @param [in] lb_a parameter matrix for constraint lb_a < a*u < ub_a (not used here)
+   * @param [in] ub_a parameter matrix for constraint lb_a < a*u < ub_a (not used here)
+   * @param [out] u optimal variable vector
+   * @return true if the problem was solved
+   */
+  bool solve(
+    const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
+    const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
+    const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override;
+
+private:
+  autoware::common::osqp::OSQPInterface osqpsolver_;
+//  rclcpp::Logger logger_;
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_

src/decition/mpccontroller/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp

@@ -0,0 +1,58 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
+#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
+
+#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/**
+ * Solver for QP problems using least square decomposition
+ * implement with Eigen's standard Cholesky decomposition (LLT)
+ */
+class QPSolverEigenLeastSquareLLT : public QPSolverInterface
+{
+public:
+  /**
+   * @brief constructor
+   */
+  QPSolverEigenLeastSquareLLT();
+
+  /**
+   * @brief destructor
+   */
+  ~QPSolverEigenLeastSquareLLT() = default;
+
+  /**
+   * @brief solve QP problem : minimize j = u' * h_mat * u + f_vec' * u without constraint
+   * @param [in] h_mat parameter matrix in object function
+   * @param [in] f_vec parameter matrix in object function
+   * @param [in] a parameter matrix for constraint lb_a < a*u < ub_a (not used here)
+   * @param [in] lb parameter matrix for constraint lb < U < ub (not used here)
+   * @param [in] ub parameter matrix for constraint lb < U < ub (not used here)
+   * @param [in] lb_a parameter matrix for constraint lb_a < a*u < ub_a (not used here)
+   * @param [in] ub_a parameter matrix for constraint lb_a < a*u < ub_a (not used here)
+   * @param [out] u optimal variable vector
+   * @return true if the problem was solved
+   */
+  bool solve(
+    const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
+    const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
+    const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override;
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_

src/decition/mpccontroller/mpc_lateral_controller/steering_offset/steering_offset.hpp

@@ -0,0 +1,49 @@
+// Copyright 2018-2023 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_
+#define MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_
+
+//#include <geometry_msgs/msg/twist.hpp>
+
+#include <cmath>
+#include <deque>
+#include <vector>
+
+class SteeringOffsetEstimator
+{
+public:
+  SteeringOffsetEstimator(
+    double wheelbase, double average_num, double vel_thres, double steer_thres,
+    double offset_limit);
+  ~SteeringOffsetEstimator() = default;
+
+  double getOffset() const;
+//  void updateOffset(const geometry_msgs::msg::Twist & twist, const double steering);
+  void updateOffset(const double vel, const double yaw_v,  const double steering);
+
+private:
+  // parameters
+  double wheelbase_ = 3.0;
+  size_t average_num_ = 1000;
+  double update_vel_threshold_ = 8.0;
+  double update_steer_threshold_ = 0.05;
+  double offset_limit_ = 0.02;
+
+  // results
+  std::deque<double> steering_offset_storage_;
+  double steering_offset_ = 0.0;
+};
+
+#endif  // MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_

src/decition/mpccontroller/mpc_lateral_controller/steering_predictor.hpp

@@ -0,0 +1,97 @@
+// Copyright 2023 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_
+#define MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_
+
+//#include "rclcpp/rclcpp.hpp"
+
+//#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
+
+#include <memory>
+#include <vector>
+#include <cmath>
+
+class ADCAckermannLateralCommand
+{
+public:
+    double stamp;
+    double steering_tire_angle;
+};
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+//using autoware_auto_control_msgs::msg::AckermannLateralCommand;
+
+class SteeringPredictor
+{
+public:
+  SteeringPredictor(const double steer_tau, const double steer_delay);
+  ~SteeringPredictor() = default;
+
+  /**
+   * @brief Calculate the predicted steering based on the given vehicle model.
+   * @return The predicted steering angle.
+   */
+  double calcSteerPrediction();
+
+  /**
+   * @brief Store the steering command in the buffer.
+   * @param steer The steering command to be stored.
+   */
+  void storeSteerCmd(const double steer);
+
+private:
+ // rclcpp::Logger m_logger = rclcpp::get_logger("mpc_steer_predictor");
+//  rclcpp::Clock::SharedPtr m_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  // The previously predicted steering value.
+  double m_steer_prediction_prev = 0.0;
+
+  // Previous computation time.
+ // rclcpp::Time m_time_prev = rclcpp::Time(0, 0, RCL_ROS_TIME);
+  double m_time_prev;
+
+  // time constant for the steering dynamics
+  double m_steer_tau;
+
+  // time delay for the steering dynamics
+  double m_input_delay;
+
+  // Buffer of sent control commands.
+  std::vector<ADCAckermannLateralCommand> m_ctrl_cmd_vec;
+
+  /**
+   * @brief Get the sum of all steering commands over the given time range.
+   * @param t_start The start time of the range.
+   * @param t_end The end time of the range.
+   * @param time_constant The time constant for the sum calculation.
+   * @return The sum of the steering commands.
+   */
+//  double getSteerCmdSum(
+//    const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const;
+
+
+  double getSteerCmdSum(
+      const double  & t_start, const double & t_end, const double time_constant) const;
+
+  /**
+   * @brief Set previously calculated steering
+   */
+  void setPrevResult(const double & steering);
+};
+
+}  // namespace autoware::motion::control::mpc_lateral_controller
+
+#endif  // MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_

src/decition/mpccontroller/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp

@@ -0,0 +1,115 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ *    Representation
+ * e      : lateral error
+ * de     : derivative of lateral error
+ * th     : heading angle error
+ * dth    : derivative of heading angle error
+ * steer  : steering angle (input)
+ * v      : velocity
+ * m      : mass
+ * Iz     : inertia
+ * lf     : length from center to front tire
+ * lr     : length from center to rear tire
+ * cf     : front tire cornering power
+ * cr     : rear tire cornering power
+ * k      : curvature on reference trajectory point
+ *
+ *    State & Input
+ * x = [e, de, th, dth]^T
+ * u = steer
+ *
+ *    Linearized model around reference point (v=vr)
+ *          [0,                   1,                0,                        0]       [       0] [
+ * 0] dx/dt = [0,
+ * -(cf+cr)/m/vr,        (cf+cr)/m,       (lr*cr-lf*cf)/m/vr] * x + [    cf/m] * u +
+ * [(lr*cr-lf*cf)/m/vr*k - vr*k] [0, 0,                0,                        1]       [       0]
+ * [                          0] [0, (lr*cr-lf*cf)/Iz/vr, (lf*cf-lr*cr)/Iz,
+ * -(lf^2*cf+lr^2*cr)/Iz/vr]       [lf*cf/Iz]       [   -(lf^2*cf+lr^2*cr)/Iz/vr]
+ *
+ * Reference : Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path
+ * Tracking", Robotics Institute, Carnegie Mellon University, February 2009.
+ */
+
+#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
+#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
+
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <string>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/**
+ * Vehicle model class of bicycle dynamics
+ * @brief calculate model-related values
+ */
+class DynamicsBicycleModel : public VehicleModelInterface
+{
+public:
+  /**
+   * @brief constructor with parameter initialization
+   * @param [in] wheelbase wheelbase length [m]
+   * @param [in] mass_fl mass applied to front left tire [kg]
+   * @param [in] mass_fr mass applied to front right tire [kg]
+   * @param [in] mass_rl mass applied to rear left tire [kg]
+   * @param [in] mass_rr mass applied to rear right tire [kg]
+   * @param [in] cf front cornering power [N/rad]
+   * @param [in] cr rear cornering power [N/rad]
+   */
+  DynamicsBicycleModel(
+    const double wheelbase, const double mass_fl, const double mass_fr, const double mass_rl,
+    const double mass_rr, const double cf, const double cr);
+
+  /**
+   * @brief destructor
+   */
+  ~DynamicsBicycleModel() = default;
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk
+   * @param [in] a_d coefficient matrix
+   * @param [in] b_d coefficient matrix
+   * @param [in] c_d coefficient matrix
+   * @param [in] w_d coefficient matrix
+   * @param [in] dt Discretization time [s]
+   */
+  void calculateDiscreteMatrix(
+    Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+    const double dt) override;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] u_ref input
+   */
+  void calculateReferenceInput(Eigen::MatrixXd & u_ref) override;
+
+  std::string modelName() override { return "dynamics"; };
+
+private:
+  double m_lf;    //!< @brief length from center of mass to front wheel [m]
+  double m_lr;    //!< @brief length from center of mass to rear wheel [m]
+  double m_mass;  //!< @brief total mass of vehicle [kg]
+  double m_iz;    //!< @brief moment of inertia [kg * m2]
+  double m_cf;    //!< @brief front cornering power [N/rad]
+  double m_cr;    //!< @brief rear cornering power [N/rad]
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_

src/decition/mpccontroller/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp

@@ -0,0 +1,99 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ *    Representation
+ * e      : lateral error
+ * th     : heading angle error
+ * steer  : steering angle
+ * steer_d: desired steering angle (input)
+ * v      : velocity
+ * W      : wheelbase length
+ * tau    : time constant for steering dynamics
+ *
+ *    State & Input
+ * x = [e, th, steer]^T
+ * u = steer_d
+ *
+ *    Nonlinear model
+ * dx1/dt = v * sin(x2)
+ * dx2/dt = v * tan(x3) / W
+ * dx3/dt = -(x3 - u) / tau
+ *
+ *    Linearized model around reference point (v = v_r, th = th_r, steer = steer_r)
+ *         [0,  vr,                   0]       [    0]       [                           0]
+ * dx/dt = [0,   0, vr/W/cos(steer_r)^2] * x + [    0] * u + [-vr*steer_r/W/cos(steer_r)^2]
+ *         [0,   0,               1/tau]       [1/tau]       [                           0]
+ *
+ */
+
+#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
+#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
+
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <string>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/**
+ * Vehicle model class of bicycle kinematics
+ * @brief calculate model-related values
+ */
+class KinematicsBicycleModel : public VehicleModelInterface
+{
+public:
+  /**
+   * @brief constructor with parameter initialization
+   * @param [in] wheelbase wheelbase length [m]
+   * @param [in] steer_lim steering angle limit [rad]
+   * @param [in] steer_tau steering time constant for 1d-model [s]
+   */
+  KinematicsBicycleModel(const double wheelbase, const double steer_lim, const double steer_tau);
+
+  /**
+   * @brief destructor
+   */
+  ~KinematicsBicycleModel() = default;
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk
+   * @param [out] a_d coefficient matrix
+   * @param [out] b_d coefficient matrix
+   * @param [out] c_d coefficient matrix
+   * @param [out] w_d coefficient matrix
+   * @param [in] dt Discretization time [s]
+   */
+  void calculateDiscreteMatrix(
+    Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+    const double dt) override;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] u_ref input
+   */
+  void calculateReferenceInput(Eigen::MatrixXd & u_ref) override;
+
+  std::string modelName() override { return "kinematics"; };
+
+private:
+  double m_steer_lim;  //!< @brief steering angle limit [rad]
+  double m_steer_tau;  //!< @brief steering time constant for 1d-model [s]
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_

src/decition/mpccontroller/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp

@@ -0,0 +1,94 @@
+// Copyright 2018-2021 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ *    Representation
+ * e      : lateral error
+ * th     : heading angle error
+ * steer  : steering angle (input)
+ * v      : velocity
+ * W      : wheelbase length
+ * tau    : time constant for steering dynamics
+ *
+ *    State & Input
+ * x = [e, th]^T
+ * u = steer
+ *
+ *    Nonlinear model
+ * dx1/dt = v * sin(x2)
+ * dx2/dt = v * tan(u) / W
+ *
+ *    Linearized model around reference point (v = v_r, th = th_r, steer = steer_r)
+ *  dx/dt = [0, vr] * x + [                  0] * u + [                           0]
+ *          [0,  0]       [vr/W/cos(steer_r)^2]       [-vr*steer_r/W/cos(steer_r)^2]
+ *
+ */
+
+#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_
+#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_
+
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <string>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/**
+ * Vehicle model class of bicycle kinematics without steering delay
+ * @brief calculate model-related values
+ */
+class KinematicsBicycleModelNoDelay : public VehicleModelInterface
+{
+public:
+  /**
+   * @brief constructor with parameter initialization
+   * @param [in] wheelbase wheelbase length [m]
+   * @param [in] steer_lim steering angle limit [rad]
+   */
+  KinematicsBicycleModelNoDelay(const double wheelbase, const double steer_lim);
+
+  /**
+   * @brief destructor
+   */
+  ~KinematicsBicycleModelNoDelay() = default;
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk
+   * @param [out] a_d coefficient matrix
+   * @param [out] b_d coefficient matrix
+   * @param [out] c_d coefficient matrix
+   * @param [out] w_d coefficient matrix
+   * @param [in] dt Discretization time [s]
+   */
+  void calculateDiscreteMatrix(
+    Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+    const double dt) override;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] u_ref input
+   */
+  void calculateReferenceInput(Eigen::MatrixXd & u_ref) override;
+
+  std::string modelName() override { return "kinematics_no_delay"; };
+
+private:
+  double m_steer_lim;  //!< @brief steering angle limit [rad]
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_

src/decition/mpccontroller/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp

@@ -0,0 +1,114 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
+#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_
+
+#include <Eigen/Core>
+
+#include <string>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+/**
+ * Vehicle model class
+ * @brief calculate model-related values
+ */
+class VehicleModelInterface
+{
+protected:
+  const int m_dim_x;   //!< @brief dimension of state x
+  const int m_dim_u;   //!< @brief dimension of input u
+  const int m_dim_y;   //!< @brief dimension of output y
+  double m_velocity;   //!< @brief vehicle velocity [m/s]
+  double m_curvature;  //!< @brief curvature on the linearized point on path
+  double m_wheelbase;  //!< @brief wheelbase of the vehicle [m]
+
+public:
+  /**
+   * @brief constructor
+   * @param [in] dim_x dimension of state x
+   * @param [in] dim_u dimension of input u
+   * @param [in] dim_y dimension of output y
+   * @param [in] wheelbase wheelbase of the vehicle [m]
+   */
+  VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase);
+
+  /**
+   * @brief destructor
+   */
+  virtual ~VehicleModelInterface() = default;
+
+  /**
+   * @brief get state x dimension
+   * @return state dimension
+   */
+  int getDimX();
+
+  /**
+   * @brief get input u dimension
+   * @return input dimension
+   */
+  int getDimU();
+
+  /**
+   * @brief get output y dimension
+   * @return output dimension
+   */
+  int getDimY();
+
+  /**
+   * @brief get wheelbase of the vehicle
+   * @return wheelbase value [m]
+   */
+  double getWheelbase();
+
+  /**
+   * @brief set velocity
+   * @param [in] velocity vehicle velocity
+   */
+  void setVelocity(const double velocity);
+
+  /**
+   * @brief set curvature
+   * @param [in] curvature curvature on the linearized point on path
+   */
+  void setCurvature(const double curvature);
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk
+   * @param [out] a_d coefficient matrix
+   * @param [out] b_d coefficient matrix
+   * @param [out] c_d coefficient matrix
+   * @param [out] w_d coefficient matrix
+   * @param [in] dt Discretization time [s]
+   */
+  virtual void calculateDiscreteMatrix(
+    Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+    const double dt) = 0;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] u_ref input
+   */
+  virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0;
+
+  /**
+   * @brief returns model name e.g. kinematics, dynamics
+   */
+  virtual std::string modelName() = 0;
+};
+}  // namespace autoware::motion::control::mpc_lateral_controller
+#endif  // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_

src/decition/mpccontroller/mpc_trajectory.cpp

@@ -0,0 +1,85 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/mpc_trajectory.hpp"
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+void MPCTrajectory::push_back(
+  const double & xp, const double & yp, const double & zp, const double & yawp, const double & vxp,
+  const double & kp, const double & smooth_kp, const double & tp)
+{
+  x.push_back(xp);
+  y.push_back(yp);
+  z.push_back(zp);
+  yaw.push_back(yawp);
+  vx.push_back(vxp);
+  k.push_back(kp);
+  smooth_k.push_back(smooth_kp);
+  relative_time.push_back(tp);
+}
+
+void MPCTrajectory::push_back(const MPCTrajectoryPoint & p)
+{
+  x.push_back(p.x);
+  y.push_back(p.y);
+  z.push_back(p.z);
+  yaw.push_back(p.yaw);
+  vx.push_back(p.vx);
+  k.push_back(p.k);
+  smooth_k.push_back(p.smooth_k);
+  relative_time.push_back(p.relative_time);
+}
+
+MPCTrajectoryPoint MPCTrajectory::back()
+{
+  MPCTrajectoryPoint p;
+
+  p.x = x.back();
+  p.y = y.back();
+  p.z = z.back();
+  p.yaw = yaw.back();
+  p.vx = vx.back();
+  p.k = k.back();
+  p.smooth_k = smooth_k.back();
+  p.relative_time = relative_time.back();
+
+  return p;
+}
+
+void MPCTrajectory::clear()
+{
+  x.clear();
+  y.clear();
+  z.clear();
+  yaw.clear();
+  vx.clear();
+  k.clear();
+  smooth_k.clear();
+  relative_time.clear();
+}
+
+size_t MPCTrajectory::size() const
+{
+  if (
+    x.size() == y.size() && x.size() == z.size() && x.size() == yaw.size() &&
+    x.size() == vx.size() && x.size() == k.size() && x.size() == smooth_k.size() &&
+    x.size() == relative_time.size()) {
+    return x.size();
+  } else {
+    std::cerr << "[MPC trajectory] trajectory size is inappropriate" << std::endl;
+    return 0;
+  }
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/mpc_utils.cpp

@@ -0,0 +1,462 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/mpc_utils.hpp"
+
+#include "interpolation/linear_interpolation.hpp"
+#include "interpolation/spline_interpolation.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/math/normalization.hpp"
+
+#include <algorithm>
+#include <limits>
+#include <string>
+#include <vector>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+namespace
+{
+double calcLongitudinalOffset(
+  const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back,
+  const geometry_msgs::msg::Point & p_target)
+{
+  const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0};
+  const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0};
+
+  return segment_vec.dot(target_vec) / segment_vec.norm();
+}
+}  // namespace
+
+namespace MPCUtils
+{
+//using tier4_autoware_utils::calcDistance2d;
+//using tier4_autoware_utils::createQuaternionFromYaw;
+//using tier4_autoware_utils::normalizeRadian;
+
+double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2)
+{
+  const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2);
+  const double dy = trajectory.y.at(idx1) - trajectory.y.at(idx2);
+  return std::hypot(dx, dy);
+}
+
+double calcDistance3d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2)
+{
+  const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2);
+  const double dy = trajectory.y.at(idx1) - trajectory.y.at(idx2);
+  const double dz = trajectory.z.at(idx1) - trajectory.z.at(idx2);
+  return std::hypot(dx, dy, dz);
+}
+
+void convertEulerAngleToMonotonic(std::vector<double> & angle_vector)
+{
+  for (uint i = 1; i < angle_vector.size(); ++i) {
+    const double da = angle_vector.at(i) - angle_vector.at(i - 1);
+    angle_vector.at(i) = angle_vector.at(i - 1) + normalizeRadian(da);
+  }
+}
+
+double calcLateralError(const Pose & ego_pose, const Pose & ref_pose)
+{
+  const double err_x = ego_pose.position.x - ref_pose.position.x;
+  const double err_y = ego_pose.position.y - ref_pose.position.y;
+  const double ref_yaw = tf2::getYaw(ref_pose.orientation);
+  const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y;
+  return lat_err;
+}
+
+void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<double> & arc_length)
+{
+  double dist = 0.0;
+  arc_length.clear();
+  arc_length.push_back(dist);
+  for (uint i = 1; i < trajectory.size(); ++i) {
+    dist += calcDistance2d(trajectory, i, i - 1);
+    arc_length.push_back(dist);
+  }
+}
+
+std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
+  const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx,
+  const double ego_offset_to_segment)
+{
+  MPCTrajectory output;
+
+  if (input.empty()) {
+    return {true, output};
+  }
+  std::vector<double> input_arclength;
+  calcMPCTrajectoryArcLength(input, input_arclength);
+
+  if (input_arclength.empty()) {
+    return {false, output};
+  }
+
+  std::vector<double> output_arclength;
+  // To accurately sample the ego point, resample separately in the forward direction and the
+  // backward direction from the current position.
+  for (double s = std::clamp(
+         input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0,
+         input_arclength.back() - 1e-6);
+       0 <= s; s -= resample_interval_dist) {
+    output_arclength.push_back(s);
+  }
+  std::reverse(output_arclength.begin(), output_arclength.end());
+  for (double s = std::max(input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0) +
+                  resample_interval_dist;
+       s < input_arclength.back(); s += resample_interval_dist) {
+    output_arclength.push_back(s);
+  }
+
+  std::vector<double> input_yaw = input.yaw;
+  convertEulerAngleToMonotonic(input_yaw);
+
+  const auto lerp_arc_length = [&](const auto & input_value) {
+    return interpolation::lerp(input_arclength, input_value, output_arclength);
+  };
+  const auto spline_arc_length = [&](const auto & input_value) {
+    return interpolation::spline(input_arclength, input_value, output_arclength);
+  };
+
+  output.x = spline_arc_length(input.x);
+  output.y = spline_arc_length(input.y);
+  output.z = spline_arc_length(input.z);
+  output.yaw = spline_arc_length(input.yaw);
+  output.vx = lerp_arc_length(input.vx);  // must be linear
+  output.k = spline_arc_length(input.k);
+  output.smooth_k = spline_arc_length(input.smooth_k);
+  output.relative_time = lerp_arc_length(input.relative_time);  // must be linear
+
+  return {true, output};
+}
+
+bool linearInterpMPCTrajectory(
+  const std::vector<double> & in_index, const MPCTrajectory & in_traj,
+  const std::vector<double> & out_index, MPCTrajectory & out_traj)
+{
+  if (in_traj.empty()) {
+    out_traj = in_traj;
+    return true;
+  }
+
+  std::vector<double> in_traj_yaw = in_traj.yaw;
+  convertEulerAngleToMonotonic(in_traj_yaw);
+
+  const auto lerp_arc_length = [&](const auto & input_value) {
+    return interpolation::lerp(in_index, input_value, out_index);
+  };
+
+  try {
+    out_traj.x = lerp_arc_length(in_traj.x);
+    out_traj.y = lerp_arc_length(in_traj.y);
+    out_traj.z = lerp_arc_length(in_traj.z);
+    out_traj.yaw = lerp_arc_length(in_traj.yaw);
+    out_traj.vx = lerp_arc_length(in_traj.vx);
+    out_traj.k = lerp_arc_length(in_traj.k);
+    out_traj.smooth_k = lerp_arc_length(in_traj.smooth_k);
+    out_traj.relative_time = lerp_arc_length(in_traj.relative_time);
+  } catch (const std::exception & e) {
+    std::cerr << "linearInterpMPCTrajectory error!: " << e.what() << std::endl;
+  }
+
+  if (out_traj.empty()) {
+    std::cerr << "[mpc util] linear interpolation error" << std::endl;
+    return false;
+  }
+
+  return true;
+}
+
+void calcTrajectoryYawFromXY(MPCTrajectory & traj, const bool is_forward_shift)
+{
+  if (traj.yaw.size() < 3) {  // at least 3 points are required to calculate yaw
+    return;
+  }
+  if (traj.yaw.size() != traj.vx.size()) {
+    RCLCPP_ERROR(rclcpp::get_logger("mpc_utils"), "trajectory size has no consistency.");
+    return;
+  }
+
+  // interpolate yaw
+  for (int i = 1; i < static_cast<int>(traj.yaw.size()) - 1; ++i) {
+    const double dx = traj.x.at(i + 1) - traj.x.at(i - 1);
+    const double dy = traj.y.at(i + 1) - traj.y.at(i - 1);
+    traj.yaw.at(i) = is_forward_shift ? std::atan2(dy, dx) : std::atan2(dy, dx) + M_PI;
+  }
+  if (traj.yaw.size() > 1) {
+    traj.yaw.at(0) = traj.yaw.at(1);
+    traj.yaw.back() = traj.yaw.at(traj.yaw.size() - 2);
+  }
+}
+
+void calcTrajectoryCurvature(
+  const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer,
+  MPCTrajectory & traj)
+{
+  traj.k = calcTrajectoryCurvature(curvature_smoothing_num_traj, traj);
+  traj.smooth_k = calcTrajectoryCurvature(curvature_smoothing_num_ref_steer, traj);
+}
+
+std::vector<double> calcTrajectoryCurvature(
+  const int curvature_smoothing_num, const MPCTrajectory & traj)
+{
+  std::vector<double> curvature_vec(traj.x.size());
+
+  /* calculate curvature by circle fitting from three points */
+  geometry_msgs::msg::Point p1, p2, p3;
+  const int max_smoothing_num =
+    static_cast<int>(std::floor(0.5 * (static_cast<double>(traj.x.size() - 1))));
+  const size_t L = static_cast<size_t>(std::min(curvature_smoothing_num, max_smoothing_num));
+  for (size_t i = L; i < traj.x.size() - L; ++i) {
+    const size_t curr_idx = i;
+    const size_t prev_idx = curr_idx - L;
+    const size_t next_idx = curr_idx + L;
+    p1.x = traj.x.at(prev_idx);
+    p2.x = traj.x.at(curr_idx);
+    p3.x = traj.x.at(next_idx);
+    p1.y = traj.y.at(prev_idx);
+    p2.y = traj.y.at(curr_idx);
+    p3.y = traj.y.at(next_idx);
+    try {
+      curvature_vec.at(curr_idx) = tier4_autoware_utils::calcCurvature(p1, p2, p3);
+    } catch (...) {
+      std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl;
+      curvature_vec.at(curr_idx) = 0.0;
+    }
+  }
+
+  /* first and last curvature is copied from next value */
+  for (size_t i = 0; i < std::min(L, traj.x.size()); ++i) {
+    curvature_vec.at(i) = curvature_vec.at(std::min(L, traj.x.size() - 1));
+    curvature_vec.at(traj.x.size() - i - 1) =
+      curvature_vec.at(std::max(traj.x.size() - L - 1, size_t(0)));
+  }
+  return curvature_vec;
+}
+
+MPCTrajectory convertToMPCTrajectory(const Trajectory & input)
+{
+  MPCTrajectory output;
+  for (const TrajectoryPoint & p : input.points) {
+    const double x = p.pose.position.x;
+    const double y = p.pose.position.y;
+    const double z = p.pose.position.z;
+    const double yaw = tf2::getYaw(p.pose.orientation);
+    const double vx = p.longitudinal_velocity_mps;
+    const double k = 0.0;
+    const double t = 0.0;
+    output.push_back(x, y, z, yaw, vx, k, k, t);
+  }
+  calcMPCTrajectoryTime(output);
+  return output;
+}
+
+Trajectory convertToAutowareTrajectory(const MPCTrajectory & input)
+{
+  Trajectory output;
+  TrajectoryPoint p;
+  for (size_t i = 0; i < input.size(); ++i) {
+    p.pose.position.x = input.x.at(i);
+    p.pose.position.y = input.y.at(i);
+    p.pose.position.z = input.z.at(i);
+    p.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(input.yaw.at(i));
+    p.longitudinal_velocity_mps =
+      static_cast<decltype(p.longitudinal_velocity_mps)>(input.vx.at(i));
+    output.points.push_back(p);
+    if (output.points.size() == output.points.max_size()) {
+      break;
+    }
+  }
+  return output;
+}
+
+bool calcMPCTrajectoryTime(MPCTrajectory & traj)
+{
+  constexpr auto min_dt = 1.0e-4;  // must be positive value to avoid duplication in time
+  double t = 0.0;
+  traj.relative_time.clear();
+  traj.relative_time.push_back(t);
+  for (size_t i = 0; i < traj.x.size() - 1; ++i) {
+    const double dist = calcDistance3d(traj, i, i + 1);
+    const double v = std::max(std::fabs(traj.vx.at(i)), 0.1);
+    t += std::max(dist / v, min_dt);
+    traj.relative_time.push_back(t);
+  }
+  return true;
+}
+
+void dynamicSmoothingVelocity(
+  const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
+  MPCTrajectory & traj)
+{
+  double curr_v = start_vel;
+  // set current velocity in both start and end point of the segment
+  traj.vx.at(start_seg_idx) = start_vel;
+  if (1 < traj.vx.size()) {
+    traj.vx.at(start_seg_idx + 1) = start_vel;
+  }
+
+  for (size_t i = start_seg_idx + 2; i < traj.size(); ++i) {
+    const double ds = calcDistance2d(traj, i, i - 1);
+    const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits<double>::epsilon());
+    const double a = tau / std::max(tau + dt, std::numeric_limits<double>::epsilon());
+    const double updated_v = a * curr_v + (1.0 - a) * traj.vx.at(i);
+    const double dv = std::max(-acc_lim * dt, std::min(acc_lim * dt, updated_v - curr_v));
+    curr_v = curr_v + dv;
+    traj.vx.at(i) = curr_v;
+  }
+  calcMPCTrajectoryTime(traj);
+}
+
+bool calcNearestPoseInterp(
+  const MPCTrajectory & traj, const Pose & self_pose, Pose * nearest_pose, size_t * nearest_index,
+  double * nearest_time, const double max_dist, const double max_yaw)
+{
+  if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) {
+    return false;
+  }
+
+  const auto autoware_traj = convertToAutowareTrajectory(traj);
+  if (autoware_traj.points.empty()) {
+    const auto logger = rclcpp::get_logger("mpc_util");
+    auto clock = rclcpp::Clock(RCL_ROS_TIME);
+    RCLCPP_WARN_THROTTLE(logger, clock, 5000, "[calcNearestPoseInterp] input trajectory is empty");
+    return false;
+  }
+
+  *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    autoware_traj.points, self_pose, max_dist, max_yaw);
+  const size_t traj_size = traj.size();
+
+  if (traj.size() == 1) {
+    nearest_pose->position.x = traj.x.at(*nearest_index);
+    nearest_pose->position.y = traj.y.at(*nearest_index);
+    nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index));
+    *nearest_time = traj.relative_time.at(*nearest_index);
+    return true;
+  }
+
+  /* get second nearest index = next to nearest_index */
+  const auto [prev, next] = [&]() -> std::pair<size_t, size_t> {
+    if (*nearest_index == 0) {
+      return std::make_pair(0, 1);
+    }
+    if (*nearest_index == traj_size - 1) {
+      return std::make_pair(traj_size - 2, traj_size - 1);
+    }
+
+    geometry_msgs::msg::Point nearest_traj_point;
+    nearest_traj_point.x = traj.x.at(*nearest_index);
+    nearest_traj_point.y = traj.y.at(*nearest_index);
+    geometry_msgs::msg::Point next_nearest_traj_point;
+    next_nearest_traj_point.x = traj.x.at(*nearest_index + 1);
+    next_nearest_traj_point.y = traj.y.at(*nearest_index + 1);
+
+    const double signed_length =
+      calcLongitudinalOffset(nearest_traj_point, next_nearest_traj_point, self_pose.position);
+    if (signed_length <= 0) {
+      return std::make_pair(*nearest_index - 1, *nearest_index);
+    }
+    return std::make_pair(*nearest_index, *nearest_index + 1);
+  }();
+
+  geometry_msgs::msg::Point next_traj_point;
+  next_traj_point.x = traj.x.at(next);
+  next_traj_point.y = traj.y.at(next);
+  geometry_msgs::msg::Point prev_traj_point;
+  prev_traj_point.x = traj.x.at(prev);
+  prev_traj_point.y = traj.y.at(prev);
+  const double traj_seg_length =
+    tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point);
+  /* if distance between two points are too close */
+  if (traj_seg_length < 1.0E-5) {
+    nearest_pose->position.x = traj.x.at(*nearest_index);
+    nearest_pose->position.y = traj.y.at(*nearest_index);
+    nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index));
+    *nearest_time = traj.relative_time.at(*nearest_index);
+    return true;
+  }
+
+  /* linear interpolation */
+  const double ratio = std::clamp(
+    calcLongitudinalOffset(prev_traj_point, next_traj_point, self_pose.position) / traj_seg_length,
+    0.0, 1.0);
+  nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next);
+  nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next);
+  const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next));
+  const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err);
+  nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw);
+  *nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next);
+  return true;
+}
+
+double calcStopDistance(const Trajectory & current_trajectory, const int origin)
+{
+  constexpr float zero_velocity = std::numeric_limits<float>::epsilon();
+  const float origin_velocity =
+    current_trajectory.points.at(static_cast<size_t>(origin)).longitudinal_velocity_mps;
+  double stop_dist = 0.0;
+
+  // search forward
+  if (std::fabs(origin_velocity) > zero_velocity) {
+    for (int i = origin + 1; i < static_cast<int>(current_trajectory.points.size()) - 1; ++i) {
+      const auto & p0 = current_trajectory.points.at(i);
+      const auto & p1 = current_trajectory.points.at(i - 1);
+      stop_dist += calcDistance2d(p0, p1);
+      if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) {
+        break;
+      }
+    }
+    return stop_dist;
+  }
+
+  // search backward
+  for (int i = origin - 1; 0 < i; --i) {
+    const auto & p0 = current_trajectory.points.at(i);
+    const auto & p1 = current_trajectory.points.at(i + 1);
+    if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) {
+      break;
+    }
+    stop_dist -= calcDistance2d(p0, p1);
+  }
+  return stop_dist;
+}
+
+void extendTrajectoryInYawDirection(
+  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj)
+{
+  // set terminal yaw
+  traj.yaw.back() = yaw;
+
+  // get terminal pose
+  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(traj);
+  auto extended_pose = autoware_traj.points.back().pose;
+
+  constexpr double extend_dist = 10.0;
+  constexpr double extend_vel = 10.0;
+  const double x_offset = is_forward_shift ? interval : -interval;
+  const double dt = interval / extend_vel;
+  const size_t num_extended_point = static_cast<size_t>(extend_dist / interval);
+  for (size_t i = 0; i < num_extended_point; ++i) {
+    extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0);
+    traj.push_back(
+      extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(),
+      extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt);
+  }
+}
+
+}  // namespace MPCUtils
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/mpccontroller.cpp

@@ -0,0 +1,5 @@
+#include "mpccontroller.h"
+
+Mpccontroller::Mpccontroller()
+{
+}

src/decition/mpccontroller/mpccontroller.h

@@ -0,0 +1,12 @@
+#ifndef MPCCONTROLLER_H
+#define MPCCONTROLLER_H
+
+#include "mpccontroller_global.h"
+
+class MPCCONTROLLER_EXPORT Mpccontroller
+{
+public:
+    Mpccontroller();
+};
+
+#endif // MPCCONTROLLER_H

src/decition/mpccontroller/mpccontroller.pro

@@ -0,0 +1,23 @@
+CONFIG -= qt
+
+TEMPLATE = lib
+DEFINES += MPCCONTROLLER_LIBRARY
+
+CONFIG += c++17
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    mpccontroller.cpp
+
+HEADERS += \
+    mpccontroller_global.h \
+    mpccontroller.h
+
+# Default rules for deployment.
+unix {
+    target.path = /usr/lib
+}
+!isEmpty(target.path): INSTALLS += target

src/decition/mpccontroller/mpccontroller_global.h

@@ -0,0 +1,18 @@
+#ifndef MPCCONTROLLER_GLOBAL_H
+#define MPCCONTROLLER_GLOBAL_H
+
+#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+#  define Q_DECL_EXPORT __declspec(dllexport)
+#  define Q_DECL_IMPORT __declspec(dllimport)
+#else
+#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
+#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
+#endif
+
+#if defined(MPCCONTROLLER_LIBRARY)
+#  define MPCCONTROLLER_EXPORT Q_DECL_EXPORT
+#else
+#  define MPCCONTROLLER_EXPORT Q_DECL_IMPORT
+#endif
+
+#endif // MPCCONTROLLER_GLOBAL_H

src/decition/mpccontroller/osqp_interface.cpp

@@ -0,0 +1,443 @@
+// Copyright 2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "osqp_interface/osqp_interface.hpp"
+
+#include "osqp/osqp.h"
+#include "osqp_interface/csc_matrix_conv.hpp"
+
+#include <chrono>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <string>
+#include <tuple>
+#include <vector>
+
+namespace autoware
+{
+namespace common
+{
+namespace osqp
+{
+OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish)
+: m_work{nullptr, OSQPWorkspaceDeleter}
+{
+  m_settings = std::make_unique<OSQPSettings>();
+  m_data = std::make_unique<OSQPData>();
+
+  if (m_settings) {
+    osqp_set_default_settings(m_settings.get());
+    m_settings->alpha = 1.6;  // Change alpha parameter
+    m_settings->eps_rel = 1.0E-4;
+    m_settings->eps_abs = eps_abs;
+    m_settings->eps_prim_inf = 1.0E-4;
+    m_settings->eps_dual_inf = 1.0E-4;
+    m_settings->warm_start = true;
+    m_settings->max_iter = 4000;
+    m_settings->verbose = false;
+    m_settings->polish = polish;
+  }
+  m_exitflag = 0;
+}
+
+OSQPInterface::OSQPInterface(
+  const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
+  const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs)
+: OSQPInterface(eps_abs)
+{
+  initializeProblem(P, A, q, l, u);
+}
+
+OSQPInterface::OSQPInterface(
+  const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<double> & q,
+  const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs)
+: OSQPInterface(eps_abs)
+{
+  initializeProblem(P, A, q, l, u);
+}
+
+OSQPInterface::~OSQPInterface()
+{
+  if (m_data->P) free(m_data->P);
+  if (m_data->A) free(m_data->A);
+}
+
+void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept
+{
+  if (ptr != nullptr) {
+    osqp_cleanup(ptr);
+  }
+}
+
+void OSQPInterface::updateP(const Eigen::MatrixXd & P_new)
+{
+  /*
+  // Transform 'P' into an 'upper trapezoidal matrix'
+  Eigen::MatrixXd P_trap = P_new.triangularView<Eigen::Upper>();
+  // Transform 'P' into a sparse matrix and extract data as dynamic arrays
+  Eigen::SparseMatrix<double> P_sparse = P_trap.sparseView();
+  double *P_val_ptr = P_sparse.valuePtr();
+  // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type)
+  c_int P_elem_N = P_sparse.nonZeros();
+  */
+  CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new);
+  osqp_update_P(
+    m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(P_csc.m_vals.size()));
+}
+
+void OSQPInterface::updateCscP(const CSC_Matrix & P_csc)
+{
+  osqp_update_P(
+    m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(P_csc.m_vals.size()));
+}
+
+void OSQPInterface::updateA(const Eigen::MatrixXd & A_new)
+{
+  /*
+  // Transform 'A' into a sparse matrix and extract data as dynamic arrays
+  Eigen::SparseMatrix<double> A_sparse = A_new.sparseView();
+  double *A_val_ptr = A_sparse.valuePtr();
+  // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type)
+  c_int A_elem_N = A_sparse.nonZeros();
+  */
+  CSC_Matrix A_csc = calCSCMatrix(A_new);
+  osqp_update_A(
+    m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(A_csc.m_vals.size()));
+  return;
+}
+
+void OSQPInterface::updateCscA(const CSC_Matrix & A_csc)
+{
+  osqp_update_A(
+    m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(A_csc.m_vals.size()));
+}
+
+void OSQPInterface::updateQ(const std::vector<double> & q_new)
+{
+  std::vector<double> q_tmp(q_new.begin(), q_new.end());
+  double * q_dyn = q_tmp.data();
+  osqp_update_lin_cost(m_work.get(), q_dyn);
+}
+
+void OSQPInterface::updateL(const std::vector<double> & l_new)
+{
+  std::vector<double> l_tmp(l_new.begin(), l_new.end());
+  double * l_dyn = l_tmp.data();
+  osqp_update_lower_bound(m_work.get(), l_dyn);
+}
+
+void OSQPInterface::updateU(const std::vector<double> & u_new)
+{
+  std::vector<double> u_tmp(u_new.begin(), u_new.end());
+  double * u_dyn = u_tmp.data();
+  osqp_update_upper_bound(m_work.get(), u_dyn);
+}
+
+void OSQPInterface::updateBounds(
+  const std::vector<double> & l_new, const std::vector<double> & u_new)
+{
+  std::vector<double> l_tmp(l_new.begin(), l_new.end());
+  std::vector<double> u_tmp(u_new.begin(), u_new.end());
+  double * l_dyn = l_tmp.data();
+  double * u_dyn = u_tmp.data();
+  osqp_update_bounds(m_work.get(), l_dyn, u_dyn);
+}
+
+void OSQPInterface::updateEpsAbs(const double eps_abs)
+{
+  m_settings->eps_abs = eps_abs;  // for default setting
+  if (m_work_initialized) {
+    osqp_update_eps_abs(m_work.get(), eps_abs);  // for current work
+  }
+}
+
+void OSQPInterface::updateEpsRel(const double eps_rel)
+{
+  m_settings->eps_rel = eps_rel;  // for default setting
+  if (m_work_initialized) {
+    osqp_update_eps_rel(m_work.get(), eps_rel);  // for current work
+  }
+}
+
+void OSQPInterface::updateMaxIter(const int max_iter)
+{
+  m_settings->max_iter = max_iter;  // for default setting
+  if (m_work_initialized) {
+    osqp_update_max_iter(m_work.get(), max_iter);  // for current work
+  }
+}
+
+void OSQPInterface::updateVerbose(const bool is_verbose)
+{
+  m_settings->verbose = is_verbose;  // for default setting
+  if (m_work_initialized) {
+    osqp_update_verbose(m_work.get(), is_verbose);  // for current work
+  }
+}
+
+void OSQPInterface::updateRhoInterval(const int rho_interval)
+{
+  m_settings->adaptive_rho_interval = rho_interval;  // for default setting
+}
+
+void OSQPInterface::updateRho(const double rho)
+{
+  m_settings->rho = rho;
+  if (m_work_initialized) {
+    osqp_update_rho(m_work.get(), rho);
+  }
+}
+
+void OSQPInterface::updateAlpha(const double alpha)
+{
+  m_settings->alpha = alpha;
+  if (m_work_initialized) {
+    osqp_update_alpha(m_work.get(), alpha);
+  }
+}
+
+void OSQPInterface::updateScaling(const int scaling)
+{
+  m_settings->scaling = scaling;
+}
+
+void OSQPInterface::updatePolish(const bool polish)
+{
+  m_settings->polish = polish;
+  if (m_work_initialized) {
+    osqp_update_polish(m_work.get(), polish);
+  }
+}
+
+void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter)
+{
+  if (polish_refine_iter < 0) {
+    std::cerr << "Polish refinement iterations must be positive" << std::endl;
+    return;
+  }
+
+  m_settings->polish_refine_iter = polish_refine_iter;
+  if (m_work_initialized) {
+    osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter);
+  }
+}
+
+void OSQPInterface::updateCheckTermination(const int check_termination)
+{
+  if (check_termination < 0) {
+    std::cerr << "Check termination must be positive" << std::endl;
+    return;
+  }
+
+  m_settings->check_termination = check_termination;
+  if (m_work_initialized) {
+    osqp_update_check_termination(m_work.get(), check_termination);
+  }
+}
+
+bool OSQPInterface::setWarmStart(
+  const std::vector<double> & primal_variables, const std::vector<double> & dual_variables)
+{
+  return setPrimalVariables(primal_variables) && setDualVariables(dual_variables);
+}
+
+bool OSQPInterface::setPrimalVariables(const std::vector<double> & primal_variables)
+{
+  if (!m_work_initialized) {
+    return false;
+  }
+
+  const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data());
+  if (result != 0) {
+    std::cerr << "Failed to set primal variables for warm start" << std::endl;
+    return false;
+  }
+
+  return true;
+}
+
+bool OSQPInterface::setDualVariables(const std::vector<double> & dual_variables)
+{
+  if (!m_work_initialized) {
+    return false;
+  }
+
+  const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data());
+  if (result != 0) {
+    std::cerr << "Failed to set dual variables for warm start" << std::endl;
+    return false;
+  }
+
+  return true;
+}
+
+int64_t OSQPInterface::initializeProblem(
+  const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
+  const std::vector<double> & l, const std::vector<double> & u)
+{
+  // check if arguments are valid
+  std::stringstream ss;
+  if (P.rows() != P.cols()) {
+    ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows()
+       << ", P.cols() = " << P.cols();
+    throw std::invalid_argument(ss.str());
+  }
+  if (P.rows() != static_cast<int>(q.size())) {
+    ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows()
+       << ", q.size() = " << q.size();
+    throw std::invalid_argument(ss.str());
+  }
+  if (P.rows() != A.cols()) {
+    ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows()
+       << ", A.cols() = " << A.cols();
+    throw std::invalid_argument(ss.str());
+  }
+  if (A.rows() != static_cast<int>(l.size())) {
+    ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows()
+       << ", l.size() = " << l.size();
+    throw std::invalid_argument(ss.str());
+  }
+  if (A.rows() != static_cast<int>(u.size())) {
+    ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows()
+       << ", u.size() = " << u.size();
+    throw std::invalid_argument(ss.str());
+  }
+
+  CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
+  CSC_Matrix A_csc = calCSCMatrix(A);
+  return initializeProblem(P_csc, A_csc, q, l, u);
+}
+
+int64_t OSQPInterface::initializeProblem(
+  CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector<double> & q, const std::vector<double> & l,
+  const std::vector<double> & u)
+{
+  // Dynamic float arrays
+  std::vector<double> q_tmp(q.begin(), q.end());
+  std::vector<double> l_tmp(l.begin(), l.end());
+  std::vector<double> u_tmp(u.begin(), u.end());
+  double * q_dyn = q_tmp.data();
+  double * l_dyn = l_tmp.data();
+  double * u_dyn = u_tmp.data();
+
+  /**********************
+   * OBJECTIVE FUNCTION
+   **********************/
+  m_param_n = static_cast<int>(q.size());
+  m_data->m = static_cast<int>(l.size());
+
+  /*****************
+   * POPULATE DATA
+   *****************/
+  m_data->n = m_param_n;
+  if (m_data->P) free(m_data->P);
+  m_data->P = csc_matrix(
+    m_data->n, m_data->n, static_cast<c_int>(P_csc.m_vals.size()), P_csc.m_vals.data(),
+    P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data());
+  m_data->q = q_dyn;
+  if (m_data->A) free(m_data->A);
+  m_data->A = csc_matrix(
+    m_data->m, m_data->n, static_cast<c_int>(A_csc.m_vals.size()), A_csc.m_vals.data(),
+    A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data());
+  m_data->l = l_dyn;
+  m_data->u = u_dyn;
+
+  // Setup workspace
+  OSQPWorkspace * workspace;
+  m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get());
+  m_work.reset(workspace);
+  m_work_initialized = true;
+
+  return m_exitflag;
+}
+
+std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
+OSQPInterface::solve()
+{
+  // Solve Problem
+  osqp_solve(m_work.get());
+
+  /********************
+   * EXTRACT SOLUTION
+   ********************/
+  double * sol_x = m_work->solution->x;
+  double * sol_y = m_work->solution->y;
+  std::vector<double> sol_primal(sol_x, sol_x + m_param_n);
+  std::vector<double> sol_lagrange_multiplier(sol_y, sol_y + m_data->m);
+
+  int64_t status_polish = m_work->info->status_polish;
+  int64_t status_solution = m_work->info->status_val;
+  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);
+
+  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()
+{
+  // Run the solver on the stored problem representation.
+  std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t> result = solve();
+  return result;
+}
+
+std::tuple<std::vector<double>, std::vector<double>, int64_t, int64_t, int64_t>
+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();
+
+  m_work.reset();
+  m_work_initialized = false;
+
+  return result;
+}
+
+void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const
+{
+  const int status = getStatus();
+  if (status == 1) {
+    // No need to log since optimization was solved.
+    return;
+  }
+
+  // create message
+  std::string output_message = "";
+  if (prefix_message != "") {
+    output_message = prefix_message + " ";
+  }
+
+  const auto status_message = getStatusMessage();
+  output_message += "Optimization failed due to " + status_message;
+
+  // log with warning
+
+  std::cout<<"osqp_interface "<<output_message.c_str()<<std::endl;
+ // RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str());
+}
+}  // namespace osqp
+}  // namespace common
+}  // namespace autoware

src/decition/mpccontroller/osqp_interface/csc_matrix_conv.hpp

@@ -0,0 +1,53 @@
+// Copyright 2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_
+#define OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_
+
+#include "osqp/glob_opts.h"  // for 'c_int' type ('long' or 'long long')
+#include "osqp_interface/visibility_control.hpp"
+
+#include <Eigen/Core>
+
+#include <vector>
+
+namespace autoware
+{
+namespace common
+{
+namespace osqp
+{
+/// \brief Compressed-Column-Sparse Matrix
+struct OSQP_INTERFACE_PUBLIC CSC_Matrix
+{
+  /// Vector of non-zero values. Ex: [4,1,1,2]
+  std::vector<c_float> m_vals;
+  /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner')
+  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;
+};
+
+/// \brief Calculate CSC matrix from Eigen matrix
+OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat);
+/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix
+OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat);
+/// \brief Print the given CSC matrix to the standard output
+OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat);
+
+}  // namespace osqp
+}  // namespace common
+}  // namespace autoware
+
+#endif  // OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_

src/decition/mpccontroller/osqp_interface/osqp_interface.hpp

@@ -0,0 +1,200 @@
+// Copyright 2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_
+#define OSQP_INTERFACE__OSQP_INTERFACE_HPP_
+
+#include "osqp/osqp.h"
+#include "osqp_interface/csc_matrix_conv.hpp"
+#include "osqp_interface/visibility_control.hpp"
+
+#include <Eigen/Core>
+//#include <rclcpp/rclcpp.hpp>
+
+#include <limits>
+#include <memory>
+#include <string>
+#include <tuple>
+#include <vector>
+
+namespace autoware
+{
+namespace common
+{
+namespace osqp
+{
+constexpr c_float INF = 1e30;
+
+/**
+ * Implementation of a native C++ interface for the OSQP solver.
+ *
+ */
+class OSQP_INTERFACE_PUBLIC OSQPInterface
+{
+private:
+  std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace *)>> m_work;
+  std::unique_ptr<OSQPSettings> m_settings;
+  std::unique_ptr<OSQPData> m_data;
+  // store last work info since work is cleaned up at every execution to prevent memory leak.
+  OSQPInfo m_latest_work_info;
+  // Number of parameters to optimize
+  int64_t m_param_n;
+  // Flag to check if the current work exists
+  bool m_work_initialized = false;
+  // Exitflag
+  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();
+
+  static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept;
+
+public:
+  /// \brief Constructor without problem formulation
+  explicit OSQPInterface(
+    const c_float eps_abs = std::numeric_limits<c_float>::epsilon(), const bool polish = true);
+  /// \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.
+  /// \param q: (n) vector defining the linear cost of the problem.
+  /// \param l: (m) vector defining the lower bound problem constraint.
+  /// \param u: (m) vector defining the upper bound problem constraint.
+  /// \param eps_abs: Absolute convergence tolerance.
+  OSQPInterface(
+    const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
+    const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs);
+  OSQPInterface(
+    const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<double> & q,
+    const std::vector<double> & l, const std::vector<double> & u, const c_float eps_abs);
+  ~OSQPInterface();
+
+  /****************
+   * OPTIMIZATION
+   ****************/
+  /// \brief Solves the stored convex quadratic program (QP) problem using the OSQP solver.
+  //
+  /// \return The function returns a tuple containing the solution as two float vectors.
+  /// \return The first element of the tuple contains the 'primal' solution.
+  /// \return The second element contains the 'lagrange multiplier' solution.
+  /// \return The third element contains an integer with solver polish status information.
+
+  /// \details How to use:
+  /// \details   1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem.
+  /// \details   2. Initialize the interface and set up the problem.
+  /// \details        osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6);
+  /// \details   3. Call the optimization function.
+  /// \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        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();
+
+  /// \brief Solves convex quadratic programs (QPs) using the OSQP solver.
+  /// \return The function returns a tuple containing the solution as two float vectors.
+  /// \return The first element of the tuple contains the 'primal' solution.
+  /// \return The second element contains the 'lagrange multiplier' solution.
+  /// \return The third element contains an integer with solver polish status information.
+  /// \details How to use:
+  /// \details   1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem.
+  /// \details   2. Initialize the interface.
+  /// \details        osqp_interface = OSQPInterface(1e-6);
+  /// \details   3. Call the optimization function with the problem formulation.
+  /// \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        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(
+    const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
+    const std::vector<double> & l, const std::vector<double> & u);
+
+  /// \brief Converts the input data and sets up the workspace object.
+  /// \param P (n,n) matrix defining relations between parameters.
+  /// \param A (m,n) matrix defining parameter constraints relative to the lower and upper bound.
+  /// \param q (n) vector defining the linear cost of the problem.
+  /// \param l (m) vector defining the lower bound problem constraint.
+  /// \param u (m) vector defining the upper bound problem constraint.
+  int64_t initializeProblem(
+    const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
+    const std::vector<double> & l, const std::vector<double> & u);
+  int64_t initializeProblem(
+    CSC_Matrix P, CSC_Matrix A, const std::vector<double> & q, const std::vector<double> & l,
+    const std::vector<double> & u);
+
+  // Setter functions for warm start
+  bool setWarmStart(
+    const std::vector<double> & primal_variables, const std::vector<double> & dual_variables);
+  bool setPrimalVariables(const std::vector<double> & primal_variables);
+  bool setDualVariables(const std::vector<double> & dual_variables);
+
+  // Updates problem parameters while keeping solution in memory.
+  //
+  // Args:
+  //   P_new: (n,n) matrix defining relations between parameters.
+  //   A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound.
+  //   q_new: (n) vector defining the linear cost of the problem.
+  //   l_new: (m) vector defining the lower bound problem constraint.
+  //   u_new: (m) vector defining the upper bound problem constraint.
+  void updateP(const Eigen::MatrixXd & P_new);
+  void updateCscP(const CSC_Matrix & P_csc);
+  void updateA(const Eigen::MatrixXd & A_new);
+  void updateCscA(const CSC_Matrix & A_csc);
+  void updateQ(const std::vector<double> & q_new);
+  void updateL(const std::vector<double> & l_new);
+  void updateU(const std::vector<double> & u_new);
+  void updateBounds(const std::vector<double> & l_new, const std::vector<double> & u_new);
+  void updateEpsAbs(const double eps_abs);
+  void updateEpsRel(const double eps_rel);
+  void updateMaxIter(const int iter);
+  void updateVerbose(const bool verbose);
+  void updateRhoInterval(const int rho_interval);
+  void updateRho(const double rho);
+  void updateAlpha(const double alpha);
+  void updateScaling(const int scaling);
+  void updatePolish(const bool polish);
+  void updatePolishRefinementIteration(const int polish_refine_iter);
+  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); }
+  /// \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);
+  }
+  /// \brief Get the status value for the latest problem solved
+  inline int64_t getStatus() const { return static_cast<int64_t>(m_latest_work_info.status_val); }
+  /// \brief Get the status polish for the latest problem solved
+  inline int64_t getStatusPolish() const
+  {
+    return static_cast<int64_t>(m_latest_work_info.status_polish);
+  }
+  /// \brief Get the runtime of the latest problem solved
+  inline double getRunTime() const { return m_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; }
+  /// \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;
+};
+
+}  // namespace osqp
+}  // namespace common
+}  // namespace autoware
+
+#endif  // OSQP_INTERFACE__OSQP_INTERFACE_HPP_

src/decition/mpccontroller/osqp_interface/visibility_control.hpp

@@ -0,0 +1,37 @@
+// Copyright 2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_
+#define OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_
+
+////////////////////////////////////////////////////////////////////////////////
+#if defined(__WIN32)
+#if defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
+#define OSQP_INTERFACE_PUBLIC __declspec(dllexport)
+#define OSQP_INTERFACE_LOCAL
+#else  // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
+#define OSQP_INTERFACE_PUBLIC __declspec(dllimport)
+#define OSQP_INTERFACE_LOCAL
+#endif  // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
+#elif defined(__linux__)
+#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default")))
+#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden")))
+#elif defined(__APPLE__)
+#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default")))
+#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden")))
+#else
+#error "Unsupported Build Configuration"
+#endif
+
+#endif  // OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_

src/decition/mpccontroller/qp_solver/qp_solver_osqp.cpp

@@ -0,0 +1,92 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
+
+#include <string>
+#include <vector>
+#include <iostream>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+//QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger}
+//{
+//}
+
+QPSolverOSQP::QPSolverOSQP()
+{
+}
+
+bool QPSolverOSQP::solve(
+  const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
+  const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
+  const Eigen::VectorXd & ub_a, Eigen::VectorXd & u)
+{
+  const Eigen::Index raw_a = a.rows();
+  const Eigen::Index col_a = a.cols();
+  const Eigen::Index dim_u = ub.size();
+  Eigen::MatrixXd Identity = Eigen::MatrixXd::Identity(dim_u, dim_u);
+
+  // convert matrix to vector for osqpsolver
+  std::vector<double> f(&f_vec(0), f_vec.data() + f_vec.cols() * f_vec.rows());
+
+  std::vector<double> lower_bound;
+  std::vector<double> upper_bound;
+
+  for (int i = 0; i < dim_u; ++i) {
+    lower_bound.push_back(lb(i));
+    upper_bound.push_back(ub(i));
+  }
+
+  for (int i = 0; i < col_a; ++i) {
+    lower_bound.push_back(lb_a(i));
+    upper_bound.push_back(ub_a(i));
+  }
+
+  Eigen::MatrixXd osqpA = Eigen::MatrixXd(dim_u + col_a, raw_a);
+  osqpA << Identity, a;
+
+  /* execute optimization */
+  auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound);
+
+  std::vector<double> U_osqp = std::get<0>(result);
+  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);
+  if (status_val != 1) {
+    std::cout<<" optimization failed : "<<osqpsolver_.getStatusMessage().c_str()<<std::endl;
+ //   RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
+    return false;
+  }
+  const auto has_nan =
+    std::any_of(U_osqp.begin(), U_osqp.end(), [](const auto v) { return std::isnan(v); });
+  if (has_nan) {
+    std::cout<<"optimization failed: result contains NaN values"<<std::endl;
+ //   RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
+    return false;
+  }
+
+  // polish status: successful (1), unperformed (0), (-1) unsuccessful
+  int status_polish = std::get<2>(result);
+  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.";
+    std::cout<<s<<" The required accuracy is met, but the solution can be inaccurate."<<std::endl;
+//    RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate.", s);
+    return true;
+  }
+  return true;
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/qp_solver/qp_solver_unconstraint_fast.cpp

@@ -0,0 +1,37 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
+
+#include <Eigen/Dense>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT()
+{
+}
+bool QPSolverEigenLeastSquareLLT::solve(
+  const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & /*a*/,
+  const Eigen::VectorXd & /*lb*/, const Eigen::VectorXd & /*ub*/, const Eigen::VectorXd & /*lb_a*/,
+  const Eigen::VectorXd & /*ub_a*/, Eigen::VectorXd & u)
+{
+  if (std::fabs(h_mat.determinant()) < 1.0E-9) {
+    return false;
+  }
+
+  u = -h_mat.llt().solve(f_vec);
+
+  return true;
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/steering_offset/steering_offset.cpp

@@ -0,0 +1,78 @@
+// Copyright 2018-2023 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/steering_offset/steering_offset.hpp"
+
+#include <algorithm>
+#include <iostream>
+#include <numeric>
+
+SteeringOffsetEstimator::SteeringOffsetEstimator(
+  double wheelbase, double average_num, double vel_thres, double steer_thres, double offset_limit)
+: wheelbase_(wheelbase),
+  average_num_(average_num),
+  update_vel_threshold_(vel_thres),
+  update_steer_threshold_(steer_thres),
+  offset_limit_(offset_limit),
+  steering_offset_storage_(average_num, 0.0)
+{
+}
+
+
+void SteeringOffsetEstimator::updateOffset(const double vel,const double yaw_v,  const double steering)
+{
+    const bool update_offset =
+        (std::abs(vel) > update_vel_threshold_ &&
+         std::abs(steering) < update_steer_threshold_);
+
+    if (!update_offset) return;
+
+    const auto steer_angvel = std::atan2(yaw_v * wheelbase_, vel);
+    const auto steer_offset = steering - steer_angvel;
+    steering_offset_storage_.push_back(steer_offset);
+    if (steering_offset_storage_.size() > average_num_) {
+        steering_offset_storage_.pop_front();
+    }
+
+    steering_offset_ =
+        std::accumulate(std::begin(steering_offset_storage_), std::end(steering_offset_storage_), 0.0) /
+        std::size(steering_offset_storage_);
+}
+
+
+//void SteeringOffsetEstimator::updateOffset(
+//  const geometry_msgs::msg::Twist & twist, const double steering)
+//{
+//  const bool update_offset =
+//    (std::abs(twist.linear.x) > update_vel_threshold_ &&
+//     std::abs(steering) < update_steer_threshold_);
+
+//  if (!update_offset) return;
+
+//  const auto steer_angvel = std::atan2(twist.angular.z * wheelbase_, twist.linear.x);
+//  const auto steer_offset = steering - steer_angvel;
+//  steering_offset_storage_.push_back(steer_offset);
+//  if (steering_offset_storage_.size() > average_num_) {
+//    steering_offset_storage_.pop_front();
+//  }
+
+//  steering_offset_ =
+//    std::accumulate(std::begin(steering_offset_storage_), std::end(steering_offset_storage_), 0.0) /
+//    std::size(steering_offset_storage_);
+//}
+
+double SteeringOffsetEstimator::getOffset() const
+{
+  return std::clamp(steering_offset_, -offset_limit_, offset_limit_);
+}

src/decition/mpccontroller/steering_predictor.cpp

@@ -0,0 +1,168 @@
+// Copyright 2023 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/steering_predictor.hpp"
+
+#include <chrono>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+
+SteeringPredictor::SteeringPredictor(const double steer_tau, const double steer_delay)
+: m_steer_tau(steer_tau), m_input_delay(steer_delay)
+{
+}
+
+double SteeringPredictor::calcSteerPrediction()
+{
+  auto t_start = m_time_prev;
+ // auto t_end = m_clock->now();
+  double t_end;
+  double t_now;
+  int64_t now = std::chrono::system_clock::now().time_since_epoch().count();
+  int64_t nmskeep = now%1000000;
+  int64_t nms = now/1000000;
+  t_now  = nmskeep;
+  t_now = t_now/1000000.0;
+  t_now = t_now + nms;
+  t_now = t_now/1000.0;
+  t_end = t_now;
+  m_time_prev = t_end;
+
+  const double duration =   (t_end - t_start);
+  const double time_constant = m_steer_tau;
+
+  const double initial_response = std::exp(-duration / time_constant) * m_steer_prediction_prev;
+
+  if (m_ctrl_cmd_vec.size() <= 2) {
+    setPrevResult(initial_response);
+    return initial_response;
+  }
+
+  const auto predicted_steering = initial_response + getSteerCmdSum(t_start, t_end, time_constant);
+  setPrevResult(predicted_steering);
+
+  return predicted_steering;
+}
+
+void SteeringPredictor::storeSteerCmd(const double steer)
+{
+  double t_now;
+  int64_t now = std::chrono::system_clock::now().time_since_epoch().count();
+  int64_t nmskeep = now%1000000;
+  int64_t nms = now/1000000;
+  t_now  = nmskeep;
+  t_now = t_now/1000000.0;
+  t_now = t_now + nms;
+  t_now = t_now/1000.0;
+  const auto time_delayed = t_now + m_input_delay;
+  ADCAckermannLateralCommand cmd;
+  cmd.stamp = time_delayed;
+  cmd.steering_tire_angle = static_cast<float>(steer);
+
+  // store published ctrl cmd
+  m_ctrl_cmd_vec.emplace_back(cmd);
+
+  if (m_ctrl_cmd_vec.size() <= 2) {
+    return;
+  }
+
+  // remove unused ctrl cmd
+  constexpr double store_time = 0.3;
+  if ((time_delayed - m_ctrl_cmd_vec.at(1).stamp) > m_input_delay + store_time) {
+    m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin());
+  }
+}
+
+//double SteeringPredictor::getSteerCmdSum(
+//  const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const
+//{
+//  if (m_ctrl_cmd_vec.size() <= 2) {
+//    return 0.0;
+//  }
+
+//  // Find first index of control command container
+//  size_t idx = 1;
+//  while (t_start > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) {
+//    if ((idx + 1) >= m_ctrl_cmd_vec.size()) {
+//      return 0.0;
+//    }
+//    ++idx;
+//  }
+
+//  // Compute steer command input response
+//  double steer_sum = 0.0;
+//  auto t = t_start;
+//  while (t_end > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) {
+//    const double duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds();
+//    t = rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp);
+//    steer_sum += (1 - std::exp(-duration / time_constant)) *
+//                 static_cast<double>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);
+//    ++idx;
+//    if (idx >= m_ctrl_cmd_vec.size()) {
+//      break;
+//    }
+//  }
+
+//  const double duration = (t_end - t).seconds();
+//  steer_sum += (1 - std::exp(-duration / time_constant)) *
+//               static_cast<double>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);
+
+//  return steer_sum;
+//}
+
+double SteeringPredictor::getSteerCmdSum(
+    const double & t_start, const double & t_end, const double time_constant) const
+{
+  if (m_ctrl_cmd_vec.size() <= 2) {
+    return 0.0;
+  }
+
+  // Find first index of control command container
+  size_t idx = 1;
+  while (t_start > m_ctrl_cmd_vec.at(idx).stamp) {
+    if ((idx + 1) >= m_ctrl_cmd_vec.size()) {
+      return 0.0;
+    }
+    ++idx;
+  }
+
+  // Compute steer command input response
+  double steer_sum = 0.0;
+  auto t = t_start;
+  while (t_end > m_ctrl_cmd_vec.at(idx).stamp) {
+  //  const double duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds();
+    const double duration = (m_ctrl_cmd_vec.at(idx).stamp) - t;
+    t = m_ctrl_cmd_vec.at(idx).stamp;
+    steer_sum += (1 - std::exp(-duration / time_constant)) *
+                 static_cast<double>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);
+    ++idx;
+    if (idx >= m_ctrl_cmd_vec.size()) {
+      break;
+    }
+  }
+
+  const double duration = (t_end - t);
+  steer_sum += (1 - std::exp(-duration / time_constant)) *
+               static_cast<double>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);
+
+  return steer_sum;
+}
+
+void SteeringPredictor::setPrevResult(const double & steering)
+{
+  m_steer_prediction_prev = steering;
+}
+
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/testmpc.pro

@@ -0,0 +1,52 @@
+QT = core
+
+CONFIG += c++17 cmdline
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        csc_matrix_conv.cpp \
+        lowpass_filter.cpp \
+        main.cpp \
+        mpc.cpp \
+        mpc_lateral_controller.cpp \
+        mpc_trajectory.cpp \
+        mpc_utils.cpp \
+        osqp_interface.cpp \
+        qp_solver/qp_solver_osqp.cpp \
+        qp_solver/qp_solver_unconstraint_fast.cpp \
+        steering_offset/steering_offset.cpp \
+        steering_predictor.cpp \
+        vehicle_model/vehicle_model_bicycle_dynamics.cpp \
+        vehicle_model/vehicle_model_bicycle_kinematics.cpp \
+        vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp \
+        vehicle_model/vehicle_model_interface.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+
+unix:INCLUDEPATH += /usr/include/eigen3
+win32:INCLUDEPATH += D:\File\soft\eigen
+
+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 \
+    mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp \
+    mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp \
+    mpc_lateral_controller/steering_offset/steering_offset.hpp \
+    mpc_lateral_controller/steering_predictor.hpp \
+    mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp \
+    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

src/decition/mpccontroller/vehicle_model/vehicle_model_bicycle_dynamics.cpp

@@ -0,0 +1,89 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
+
+#include <algorithm>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+DynamicsBicycleModel::DynamicsBicycleModel(
+  const double wheelbase, const double mass_fl, const double mass_fr, const double mass_rl,
+  const double mass_rr, const double cf, const double cr)
+: VehicleModelInterface(/* dim_x */ 4, /* dim_u */ 1, /* dim_y */ 2, wheelbase)
+{
+  const double mass_front = mass_fl + mass_fr;
+  const double mass_rear = mass_rl + mass_rr;
+
+  m_mass = mass_front + mass_rear;
+  m_lf = m_wheelbase * (1.0 - mass_front / m_mass);
+  m_lr = m_wheelbase * (1.0 - mass_rear / m_mass);
+  m_iz = m_lf * m_lf * mass_front + m_lr * m_lr * mass_rear;
+  m_cf = cf;
+  m_cr = cr;
+}
+
+void DynamicsBicycleModel::calculateDiscreteMatrix(
+  Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+  const double dt)
+{
+  /*
+   * x[k+1] = a_d*x[k] + b_d*u + w_d
+   */
+
+  const double vel = std::max(m_velocity, 0.01);
+
+  a_d = Eigen::MatrixXd::Zero(m_dim_x, m_dim_x);
+  a_d(0, 1) = 1.0;
+  a_d(1, 1) = -(m_cf + m_cr) / (m_mass * vel);
+  a_d(1, 2) = (m_cf + m_cr) / m_mass;
+  a_d(1, 3) = (m_lr * m_cr - m_lf * m_cf) / (m_mass * vel);
+  a_d(2, 3) = 1.0;
+  a_d(3, 1) = (m_lr * m_cr - m_lf * m_cf) / (m_iz * vel);
+  a_d(3, 2) = (m_lf * m_cf - m_lr * m_cr) / m_iz;
+  a_d(3, 3) = -(m_lf * m_lf * m_cf + m_lr * m_lr * m_cr) / (m_iz * vel);
+
+  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x);
+  Eigen::MatrixXd a_d_inverse = (I - dt * 0.5 * a_d).inverse();
+
+  a_d = a_d_inverse * (I + dt * 0.5 * a_d);  // bilinear discretization
+
+  b_d = Eigen::MatrixXd::Zero(m_dim_x, m_dim_u);
+  b_d(0, 0) = 0.0;
+  b_d(1, 0) = m_cf / m_mass;
+  b_d(2, 0) = 0.0;
+  b_d(3, 0) = m_lf * m_cf / m_iz;
+
+  w_d = Eigen::MatrixXd::Zero(m_dim_x, 1);
+  w_d(0, 0) = 0.0;
+  w_d(1, 0) = (m_lr * m_cr - m_lf * m_cf) / (m_mass * vel) - vel;
+  w_d(2, 0) = 0.0;
+  w_d(3, 0) = -(m_lf * m_lf * m_cf + m_lr * m_lr * m_cr) / (m_iz * vel);
+
+  b_d = (a_d_inverse * dt) * b_d;
+  w_d = (a_d_inverse * dt * m_curvature * vel) * w_d;
+
+  c_d = Eigen::MatrixXd::Zero(m_dim_y, m_dim_x);
+  c_d(0, 0) = 1.0;
+  c_d(1, 2) = 1.0;
+}
+
+void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref)
+{
+  const double vel = std::max(m_velocity, 0.01);
+  const double Kv =
+    m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase);
+  u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature;
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/vehicle_model/vehicle_model_bicycle_kinematics.cpp

@@ -0,0 +1,71 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
+
+#include <cmath>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+KinematicsBicycleModel::KinematicsBicycleModel(
+  const double wheelbase, const double steer_lim, const double steer_tau)
+: VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2, wheelbase)
+{
+  m_steer_lim = steer_lim;
+  m_steer_tau = steer_tau;
+}
+
+void KinematicsBicycleModel::calculateDiscreteMatrix(
+  Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+  const double dt)
+{
+  auto sign = [](double x) { return (x > 0.0) - (x < 0.0); };
+
+  /* Linearize delta around delta_r (reference delta) */
+  double delta_r = atan(m_wheelbase * m_curvature);
+  if (std::abs(delta_r) >= m_steer_lim) {
+    delta_r = m_steer_lim * static_cast<double>(sign(delta_r));
+  }
+  double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r));
+  double velocity = m_velocity;
+  if (std::abs(m_velocity) < 1e-04) {
+    velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1);
+  }
+
+  a_d << 0.0, velocity, 0.0, 0.0, 0.0, velocity / m_wheelbase * cos_delta_r_squared_inv, 0.0, 0.0,
+    -1.0 / m_steer_tau;
+
+  b_d << 0.0, 0.0, 1.0 / m_steer_tau;
+
+  c_d << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0;
+
+  w_d << 0.0,
+    -velocity * m_curvature +
+      velocity / m_wheelbase * (tan(delta_r) - delta_r * cos_delta_r_squared_inv),
+    0.0;
+
+  // bilinear discretization for ZOH system
+  // no discretization is needed for Cd
+  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x);
+  const Eigen::MatrixXd i_dt2a_inv = (I - dt * 0.5 * a_d).inverse();
+  a_d = i_dt2a_inv * (I + dt * 0.5 * a_d);
+  b_d = i_dt2a_inv * b_d * dt;
+  w_d = i_dt2a_inv * w_d * dt;
+}
+
+void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref)
+{
+  u_ref(0, 0) = std::atan(m_wheelbase * m_curvature);
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp

@@ -0,0 +1,62 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
+
+#include <cmath>
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay(
+  const double wheelbase, const double steer_lim)
+: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2, wheelbase)
+{
+  m_steer_lim = steer_lim;
+}
+
+void KinematicsBicycleModelNoDelay::calculateDiscreteMatrix(
+  Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
+  const double dt)
+{
+  auto sign = [](double x) { return (x > 0.0) - (x < 0.0); };
+
+  /* Linearize delta around delta_r (reference delta) */
+  double delta_r = atan(m_wheelbase * m_curvature);
+  if (std::abs(delta_r) >= m_steer_lim) {
+    delta_r = m_steer_lim * static_cast<double>(sign(delta_r));
+  }
+  double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r));
+
+  a_d << 0.0, m_velocity, 0.0, 0.0;
+
+  b_d << 0.0, m_velocity / m_wheelbase * cos_delta_r_squared_inv;
+
+  c_d << 1.0, 0.0, 0.0, 1.0;
+
+  w_d << 0.0, -m_velocity / m_wheelbase * delta_r * cos_delta_r_squared_inv;
+
+  // bilinear discretization for ZOH system
+  // no discretization is needed for Cd
+  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x);
+  const Eigen::MatrixXd i_dt2a_inv = (I - dt * 0.5 * a_d).inverse();
+  a_d = i_dt2a_inv * (I + dt * 0.5 * a_d);
+  b_d = i_dt2a_inv * b_d * dt;
+  w_d = i_dt2a_inv * w_d * dt;
+}
+
+void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ref)
+{
+  u_ref(0, 0) = std::atan(m_wheelbase * m_curvature);
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/vehicle_model/vehicle_model_interface.cpp

@@ -0,0 +1,47 @@
+// Copyright 2018-2021 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
+
+namespace autoware::motion::control::mpc_lateral_controller
+{
+VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase)
+: m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase)
+{
+}
+int VehicleModelInterface::getDimX()
+{
+  return m_dim_x;
+}
+int VehicleModelInterface::getDimU()
+{
+  return m_dim_u;
+}
+int VehicleModelInterface::getDimY()
+{
+  return m_dim_y;
+}
+double VehicleModelInterface::getWheelbase()
+{
+  return m_wheelbase;
+}
+void VehicleModelInterface::setVelocity(const double velocity)
+{
+  m_velocity = velocity;
+}
+void VehicleModelInterface::setCurvature(const double curvature)
+{
+  m_curvature = curvature;
+}
+}  // namespace autoware::motion::control::mpc_lateral_controller