add decition/adcapollompc for direct use apollo contoller. now only create folder.

src/decition/adcapollompc/Readme.MD

@@ -0,0 +1,4 @@
+1.apollo代码从内部git下载。相应pb.cc文件用protoc生成。
+2.osqp从moduliazation_thirdpartylib下载。
+3.absl同上。
+

src/decition/adcapollompc/adc_mpc_controller.cc

@@ -0,0 +1,737 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * 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 "modules/control/controller/mpc_controller.h"
+
+#include <algorithm>
+#include <iomanip>
+#include <limits>
+#include <utility>
+#include <vector>
+
+#include "Eigen/LU"
+#include "absl/strings/str_cat.h"
+#include "cyber/common/log.h"
+#include "cyber/time/clock.h"
+#include "modules/common/configs/vehicle_config_helper.h"
+#include "modules/common/math/math_utils.h"
+#include "modules/common/math/mpc_osqp.h"
+#include "modules/control/common/control_gflags.h"
+
+namespace apollo {
+namespace control {
+using apollo::common::ErrorCode;
+using apollo::common::Status;
+using apollo::common::TrajectoryPoint;
+using apollo::common::VehicleStateProvider;
+using apollo::cyber::Clock;
+using Matrix = Eigen::MatrixXd;
+using apollo::common::VehicleConfigHelper;
+
+namespace {
+
+std::string GetLogFileName() {
+  time_t raw_time;
+  char name_buffer[80];
+  std::time(&raw_time);
+
+  std::tm time_tm;
+  localtime_r(&raw_time, &time_tm);
+  strftime(name_buffer, sizeof(name_buffer),
+           "/tmp/mpc_controller_%F_%H%M%S.csv", &time_tm);
+  return std::string(name_buffer);
+}
+
+void WriteHeaders(std::ofstream &file_stream) {}
+}  // namespace
+
+MPCController::MPCController() : name_("MPC Controller") {
+  if (FLAGS_enable_csv_debug) {
+    mpc_log_file_.open(GetLogFileName());
+    mpc_log_file_ << std::fixed;
+    mpc_log_file_ << std::setprecision(6);
+    WriteHeaders(mpc_log_file_);
+  }
+  AINFO << "Using " << name_;
+}
+
+MPCController::~MPCController() { CloseLogFile(); }
+
+bool MPCController::LoadControlConf(const ControlConf *control_conf) {
+  if (!control_conf) {
+    AERROR << "[MPCController] control_conf = nullptr";
+    return false;
+  }
+  vehicle_param_ = VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
+
+  ts_ = control_conf->mpc_controller_conf().ts();
+  if (ts_ <= 0.0) {
+    AERROR << "[MPCController] Invalid control update interval.";
+    return false;
+  }
+  cf_ = control_conf->mpc_controller_conf().cf();
+  cr_ = control_conf->mpc_controller_conf().cr();
+  wheelbase_ = vehicle_param_.wheel_base();
+  steer_ratio_ = vehicle_param_.steer_ratio();
+  steer_single_direction_max_degree_ =
+      vehicle_param_.max_steer_angle() * 180 / M_PI;
+  max_lat_acc_ = control_conf->mpc_controller_conf().max_lateral_acceleration();
+
+  // TODO(Shu, Qi, Yu): add sanity check for conf values
+  // steering ratio should be positive
+  static constexpr double kEpsilon = 1e-6;
+  if (std::isnan(steer_ratio_) || steer_ratio_ < kEpsilon) {
+    AERROR << "[MPCController] steer_ratio = 0";
+    return false;
+  }
+  wheel_single_direction_max_degree_ =
+      steer_single_direction_max_degree_ / steer_ratio_ / 180 * M_PI;
+  max_acceleration_ = vehicle_param_.max_acceleration();
+  max_deceleration_ = vehicle_param_.max_deceleration();
+
+  const double mass_fl = control_conf->mpc_controller_conf().mass_fl();
+  const double mass_fr = control_conf->mpc_controller_conf().mass_fr();
+  const double mass_rl = control_conf->mpc_controller_conf().mass_rl();
+  const double mass_rr = control_conf->mpc_controller_conf().mass_rr();
+  const double mass_front = mass_fl + mass_fr;
+  const double mass_rear = mass_rl + mass_rr;
+  mass_ = mass_front + mass_rear;
+
+  lf_ = wheelbase_ * (1.0 - mass_front / mass_);
+  lr_ = wheelbase_ * (1.0 - mass_rear / mass_);
+  iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;
+
+  mpc_eps_ = control_conf->mpc_controller_conf().eps();
+  mpc_max_iteration_ = control_conf->mpc_controller_conf().max_iteration();
+  throttle_lowerbound_ =
+      std::max(vehicle_param_.throttle_deadzone(),
+               control_conf->mpc_controller_conf().throttle_minimum_action());
+  brake_lowerbound_ =
+      std::max(vehicle_param_.brake_deadzone(),
+               control_conf->mpc_controller_conf().brake_minimum_action());
+
+  minimum_speed_protection_ = control_conf->minimum_speed_protection();
+  max_acceleration_when_stopped_ =
+      control_conf->max_acceleration_when_stopped();
+  max_abs_speed_when_stopped_ = vehicle_param_.max_abs_speed_when_stopped();
+  standstill_acceleration_ =
+      control_conf->mpc_controller_conf().standstill_acceleration();
+
+  enable_mpc_feedforward_compensation_ =
+      control_conf->mpc_controller_conf().enable_mpc_feedforward_compensation();
+
+  unconstrained_control_diff_limit_ =
+      control_conf->mpc_controller_conf().unconstrained_control_diff_limit();
+
+  LoadControlCalibrationTable(control_conf->mpc_controller_conf());
+  ADEBUG << "MPC conf loaded";
+  return true;
+}
+
+void MPCController::ProcessLogs(const SimpleMPCDebug *debug,
+                                const canbus::Chassis *chassis) {
+  // TODO(QiL): Add debug information
+}
+
+void MPCController::LogInitParameters() {
+  ADEBUG << name_ << " begin.";
+  ADEBUG << "[MPCController parameters]"
+         << " mass_: " << mass_ << ","
+         << " iz_: " << iz_ << ","
+         << " lf_: " << lf_ << ","
+         << " lr_: " << lr_;
+}
+
+void MPCController::InitializeFilters(const ControlConf *control_conf) {
+  // Low pass filter
+  std::vector<double> den(3, 0.0);
+  std::vector<double> num(3, 0.0);
+  common::LpfCoefficients(
+      ts_, control_conf->mpc_controller_conf().cutoff_freq(), &den, &num);
+  digital_filter_.set_coefficients(den, num);
+  lateral_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
+      control_conf->mpc_controller_conf().mean_filter_window_size()));
+  heading_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
+      control_conf->mpc_controller_conf().mean_filter_window_size()));
+}
+
+Status MPCController::Init(std::shared_ptr<DependencyInjector> injector,
+                           const ControlConf *control_conf) {
+  if (!LoadControlConf(control_conf)) {
+    AERROR << "failed to load control conf";
+    return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
+                  "failed to load control_conf");
+  }
+  injector_ = injector;
+  // Matrix init operations.
+  matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
+  matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
+  matrix_a_(0, 1) = 1.0;
+  matrix_a_(1, 2) = (cf_ + cr_) / mass_;
+  matrix_a_(2, 3) = 1.0;
+  matrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;
+  matrix_a_(4, 5) = 1.0;
+  matrix_a_(5, 5) = 0.0;
+  // TODO(QiL): expand the model to accommodate more combined states.
+
+  matrix_a_coeff_ = Matrix::Zero(basic_state_size_, basic_state_size_);
+  matrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;
+  matrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;
+  matrix_a_coeff_(2, 3) = 1.0;
+  matrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;
+  matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;
+
+  matrix_b_ = Matrix::Zero(basic_state_size_, controls_);
+  matrix_bd_ = Matrix::Zero(basic_state_size_, controls_);
+  matrix_b_(1, 0) = cf_ / mass_;
+  matrix_b_(3, 0) = lf_ * cf_ / iz_;
+  matrix_b_(4, 1) = 0.0;
+  matrix_b_(5, 1) = -1.0;
+  matrix_bd_ = matrix_b_ * ts_;
+
+  matrix_c_ = Matrix::Zero(basic_state_size_, 1);
+  matrix_cd_ = Matrix::Zero(basic_state_size_, 1);
+
+  matrix_state_ = Matrix::Zero(basic_state_size_, 1);
+  matrix_k_ = Matrix::Zero(1, basic_state_size_);
+
+  matrix_r_ = Matrix::Identity(controls_, controls_);
+
+  matrix_q_ = Matrix::Zero(basic_state_size_, basic_state_size_);
+
+  int r_param_size = control_conf->mpc_controller_conf().matrix_r_size();
+  for (int i = 0; i < r_param_size; ++i) {
+    matrix_r_(i, i) = control_conf->mpc_controller_conf().matrix_r(i);
+  }
+
+  int q_param_size = control_conf->mpc_controller_conf().matrix_q_size();
+  if (basic_state_size_ != q_param_size) {
+    const auto error_msg =
+        absl::StrCat("MPC controller error: matrix_q size: ", q_param_size,
+                     " in parameter file not equal to basic_state_size_: ",
+                     basic_state_size_);
+    AERROR << error_msg;
+    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
+  }
+  for (int i = 0; i < q_param_size; ++i) {
+    matrix_q_(i, i) = control_conf->mpc_controller_conf().matrix_q(i);
+  }
+
+  // Update matrix_q_updated_ and matrix_r_updated_
+  matrix_r_updated_ = matrix_r_;
+  matrix_q_updated_ = matrix_q_;
+
+  InitializeFilters(control_conf);
+  LoadMPCGainScheduler(control_conf->mpc_controller_conf());
+  LogInitParameters();
+  ADEBUG << "[MPCController] init done!";
+  return Status::OK();
+}
+
+void MPCController::CloseLogFile() {
+  if (FLAGS_enable_csv_debug && mpc_log_file_.is_open()) {
+    mpc_log_file_.close();
+  }
+}
+
+double MPCController::Wheel2SteerPct(const double wheel_angle) {
+  return wheel_angle / wheel_single_direction_max_degree_ * 100;
+}
+
+void MPCController::Stop() { CloseLogFile(); }
+
+std::string MPCController::Name() const { return name_; }
+
+void MPCController::LoadMPCGainScheduler(
+    const MPCControllerConf &mpc_controller_conf) {
+  const auto &lat_err_gain_scheduler =
+      mpc_controller_conf.lat_err_gain_scheduler();
+  const auto &heading_err_gain_scheduler =
+      mpc_controller_conf.heading_err_gain_scheduler();
+  const auto &feedforwardterm_gain_scheduler =
+      mpc_controller_conf.feedforwardterm_gain_scheduler();
+  const auto &steer_weight_gain_scheduler =
+      mpc_controller_conf.steer_weight_gain_scheduler();
+  ADEBUG << "MPC control gain scheduler loaded";
+  Interpolation1D::DataType xy1, xy2, xy3, xy4;
+  for (const auto &scheduler : lat_err_gain_scheduler.scheduler()) {
+    xy1.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
+  }
+  for (const auto &scheduler : heading_err_gain_scheduler.scheduler()) {
+    xy2.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
+  }
+  for (const auto &scheduler : feedforwardterm_gain_scheduler.scheduler()) {
+    xy3.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
+  }
+  for (const auto &scheduler : steer_weight_gain_scheduler.scheduler()) {
+    xy4.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
+  }
+
+  lat_err_interpolation_.reset(new Interpolation1D);
+  ACHECK(lat_err_interpolation_->Init(xy1))
+      << "Fail to load lateral error gain scheduler for MPC controller";
+
+  heading_err_interpolation_.reset(new Interpolation1D);
+  ACHECK(heading_err_interpolation_->Init(xy2))
+      << "Fail to load heading error gain scheduler for MPC controller";
+
+  feedforwardterm_interpolation_.reset(new Interpolation1D);
+  ACHECK(feedforwardterm_interpolation_->Init(xy3))
+      << "Fail to load feed forward term gain scheduler for MPC controller";
+
+  steer_weight_interpolation_.reset(new Interpolation1D);
+  ACHECK(steer_weight_interpolation_->Init(xy4))
+      << "Fail to load steer weight gain scheduler for MPC controller";
+}
+
+Status MPCController::ComputeControlCommand(
+    const localization::LocalizationEstimate *localization,
+    const canbus::Chassis *chassis,
+    const planning::ADCTrajectory *planning_published_trajectory,
+    ControlCommand *cmd) {
+  trajectory_analyzer_ =
+      std::move(TrajectoryAnalyzer(planning_published_trajectory));
+
+  SimpleMPCDebug *debug = cmd->mutable_debug()->mutable_simple_mpc_debug();
+  debug->Clear();
+
+  ComputeLongitudinalErrors(&trajectory_analyzer_, debug);
+
+  // Update state
+  UpdateState(debug);
+
+  UpdateMatrix(debug);
+
+  FeedforwardUpdate(debug);
+
+  auto vehicle_state = injector_->vehicle_state();
+  // Add gain scheduler for higher speed steering
+  if (FLAGS_enable_gain_scheduler) {
+    matrix_q_updated_(0, 0) =
+        matrix_q_(0, 0) *
+        lat_err_interpolation_->Interpolate(vehicle_state->linear_velocity());
+    matrix_q_updated_(2, 2) =
+        matrix_q_(2, 2) * heading_err_interpolation_->Interpolate(
+                              vehicle_state->linear_velocity());
+    steer_angle_feedforwardterm_updated_ =
+        steer_angle_feedforwardterm_ *
+        feedforwardterm_interpolation_->Interpolate(
+            vehicle_state->linear_velocity());
+    matrix_r_updated_(0, 0) =
+        matrix_r_(0, 0) * steer_weight_interpolation_->Interpolate(
+                              vehicle_state->linear_velocity());
+  } else {
+    matrix_q_updated_ = matrix_q_;
+    matrix_r_updated_ = matrix_r_;
+    steer_angle_feedforwardterm_updated_ = steer_angle_feedforwardterm_;
+  }
+
+  debug->add_matrix_q_updated(matrix_q_updated_(0, 0));
+  debug->add_matrix_q_updated(matrix_q_updated_(1, 1));
+  debug->add_matrix_q_updated(matrix_q_updated_(2, 2));
+  debug->add_matrix_q_updated(matrix_q_updated_(3, 3));
+
+  debug->add_matrix_r_updated(matrix_r_updated_(0, 0));
+  debug->add_matrix_r_updated(matrix_r_updated_(1, 1));
+
+  Matrix control_matrix = Matrix::Zero(controls_, 1);
+  std::vector<Matrix> control(horizon_, control_matrix);
+
+  Matrix control_gain_matrix = Matrix::Zero(controls_, basic_state_size_);
+  std::vector<Matrix> control_gain(horizon_, control_gain_matrix);
+
+  Matrix addition_gain_matrix = Matrix::Zero(controls_, 1);
+  std::vector<Matrix> addition_gain(horizon_, addition_gain_matrix);
+
+  Matrix reference_state = Matrix::Zero(basic_state_size_, 1);
+  std::vector<Matrix> reference(horizon_, reference_state);
+
+  Matrix lower_bound(controls_, 1);
+  lower_bound << -wheel_single_direction_max_degree_, max_deceleration_;
+
+  Matrix upper_bound(controls_, 1);
+  upper_bound << wheel_single_direction_max_degree_, max_acceleration_;
+
+  const double max = std::numeric_limits<double>::max();
+  Matrix lower_state_bound(basic_state_size_, 1);
+  Matrix upper_state_bound(basic_state_size_, 1);
+
+  // lateral_error, lateral_error_rate, heading_error, heading_error_rate
+  // station_error, station_error_rate
+  lower_state_bound << -1.0 * max, -1.0 * max, -1.0 * M_PI, -1.0 * max,
+      -1.0 * max, -1.0 * max;
+  upper_state_bound << max, max, M_PI, max, max, max;
+
+#ifdef ADCTEST
+  double mpc_start_timestamp = 0;
+#else
+  double mpc_start_timestamp = Clock::NowInSeconds();
+#endif
+  double steer_angle_feedback = 0.0;
+  double acc_feedback = 0.0;
+  double steer_angle_ff_compensation = 0.0;
+  double unconstrained_control_diff = 0.0;
+  double control_gain_truncation_ratio = 0.0;
+  double unconstrained_control = 0.0;
+  const double v = injector_->vehicle_state()->linear_velocity();
+
+  std::vector<double> control_cmd(controls_, 0);
+
+  apollo::common::math::MpcOsqp mpc_osqp(
+      matrix_ad_, matrix_bd_, matrix_q_updated_, matrix_r_updated_,
+      matrix_state_, lower_bound, upper_bound, lower_state_bound,
+      upper_state_bound, reference_state, mpc_max_iteration_, horizon_,
+      mpc_eps_);
+  if (!mpc_osqp.Solve(&control_cmd)) {
+    AERROR << "MPC OSQP solver failed";
+  } else {
+    ADEBUG << "MPC OSQP problem solved! ";
+    control[0](0, 0) = control_cmd.at(0);
+    control[0](1, 0) = control_cmd.at(1);
+  }
+
+  steer_angle_feedback = Wheel2SteerPct(control[0](0, 0));
+  acc_feedback = control[0](1, 0);
+  for (int i = 0; i < basic_state_size_; ++i) {
+    unconstrained_control += control_gain[0](0, i) * matrix_state_(i, 0);
+  }
+  unconstrained_control += addition_gain[0](0, 0) * v * debug->curvature();
+  if (enable_mpc_feedforward_compensation_) {
+    unconstrained_control_diff =
+        Wheel2SteerPct(control[0](0, 0) - unconstrained_control);
+    if (fabs(unconstrained_control_diff) <= unconstrained_control_diff_limit_) {
+      steer_angle_ff_compensation =
+          Wheel2SteerPct(debug->curvature() *
+                         (control_gain[0](0, 2) *
+                              (lr_ - lf_ / cr_ * mass_ * v * v / wheelbase_) -
+                          addition_gain[0](0, 0) * v));
+    } else {
+      control_gain_truncation_ratio = control[0](0, 0) / unconstrained_control;
+      steer_angle_ff_compensation =
+          Wheel2SteerPct(debug->curvature() *
+                         (control_gain[0](0, 2) *
+                              (lr_ - lf_ / cr_ * mass_ * v * v / wheelbase_) -
+                          addition_gain[0](0, 0) * v) *
+                         control_gain_truncation_ratio);
+    }
+  } else {
+    steer_angle_ff_compensation = 0.0;
+  }
+
+#ifdef ADCTEST
+  double mpc_end_timestamp = 0;
+#else
+  double mpc_end_timestamp = Clock::NowInSeconds();
+#endif
+
+  ADEBUG << "MPC core algorithm: calculation time is: "
+         << (mpc_end_timestamp - mpc_start_timestamp) * 1000 << " ms.";
+
+  // TODO(QiL): evaluate whether need to add spline smoothing after the result
+  double steer_angle = steer_angle_feedback +
+                       steer_angle_feedforwardterm_updated_ +
+                       steer_angle_ff_compensation;
+
+  if (FLAGS_set_steer_limit) {
+    const double steer_limit = std::atan(max_lat_acc_ * wheelbase_ /
+                                         (vehicle_state->linear_velocity() *
+                                          vehicle_state->linear_velocity())) *
+                               steer_ratio_ * 180 / M_PI /
+                               steer_single_direction_max_degree_ * 100;
+
+    // Clamp the steer angle with steer limitations at current speed
+    double steer_angle_limited =
+        common::math::Clamp(steer_angle, -steer_limit, steer_limit);
+    steer_angle_limited = digital_filter_.Filter(steer_angle_limited);
+    steer_angle = steer_angle_limited;
+    debug->set_steer_angle_limited(steer_angle_limited);
+  }
+  steer_angle = digital_filter_.Filter(steer_angle);
+  // Clamp the steer angle to -100.0 to 100.0
+  steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);
+  cmd->set_steering_target(steer_angle);
+
+  debug->set_acceleration_cmd_closeloop(acc_feedback);
+
+  double acceleration_cmd = acc_feedback + debug->acceleration_reference();
+  // TODO(QiL): add pitch angle feed forward to accommodate for 3D control
+
+  if ((planning_published_trajectory->trajectory_type() ==
+       apollo::planning::ADCTrajectory::NORMAL) &&
+      (std::fabs(debug->acceleration_reference()) <=
+           max_acceleration_when_stopped_ &&
+       std::fabs(debug->speed_reference()) <= max_abs_speed_when_stopped_)) {
+    acceleration_cmd =
+        (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
+            ? std::max(acceleration_cmd, -standstill_acceleration_)
+            : std::min(acceleration_cmd, standstill_acceleration_);
+    ADEBUG << "Stop location reached";
+    debug->set_is_full_stop(true);
+  }
+  // TODO(Yu): study the necessity of path_remain and add it to MPC if needed
+
+  debug->set_acceleration_cmd(acceleration_cmd);
+
+  double calibration_value = 0.0;
+  if (FLAGS_use_preview_speed_for_table) {
+    calibration_value = control_interpolation_->Interpolate(
+        std::make_pair(debug->speed_reference(), acceleration_cmd));
+  } else {
+    calibration_value = control_interpolation_->Interpolate(std::make_pair(
+        injector_->vehicle_state()->linear_velocity(), acceleration_cmd));
+  }
+
+  debug->set_calibration_value(calibration_value);
+
+  double throttle_cmd = 0.0;
+  double brake_cmd = 0.0;
+  if (calibration_value >= 0) {
+    throttle_cmd = std::max(calibration_value, throttle_lowerbound_);
+    brake_cmd = 0.0;
+  } else {
+    throttle_cmd = 0.0;
+    brake_cmd = std::max(-calibration_value, brake_lowerbound_);
+  }
+
+  cmd->set_steering_rate(FLAGS_steer_angle_rate);
+  // if the car is driven by acceleration, disgard the cmd->throttle and brake
+  cmd->set_throttle(throttle_cmd);
+  cmd->set_brake(brake_cmd);
+  cmd->set_acceleration(acceleration_cmd);
+
+  debug->set_heading(vehicle_state->heading());
+  debug->set_steering_position(chassis->steering_percentage());
+  debug->set_steer_angle(steer_angle);
+  debug->set_steer_angle_feedforward(steer_angle_feedforwardterm_updated_);
+  debug->set_steer_angle_feedforward_compensation(steer_angle_ff_compensation);
+  debug->set_steer_unconstrained_control_diff(unconstrained_control_diff);
+  debug->set_steer_angle_feedback(steer_angle_feedback);
+  debug->set_steering_position(chassis->steering_percentage());
+
+  if (std::fabs(vehicle_state->linear_velocity()) <=
+          vehicle_param_.max_abs_speed_when_stopped() ||
+      chassis->gear_location() == planning_published_trajectory->gear() ||
+      chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
+    cmd->set_gear_location(planning_published_trajectory->gear());
+  } else {
+    cmd->set_gear_location(chassis->gear_location());
+  }
+
+  ProcessLogs(debug, chassis);
+  return Status::OK();
+}
+
+Status MPCController::Reset() {
+  previous_heading_error_ = 0.0;
+  previous_lateral_error_ = 0.0;
+  return Status::OK();
+}
+
+void MPCController::LoadControlCalibrationTable(
+    const MPCControllerConf &mpc_controller_conf) {
+  const auto &control_table = mpc_controller_conf.calibration_table();
+  ADEBUG << "Control calibration table loaded";
+  ADEBUG << "Control calibration table size is "
+         << control_table.calibration_size();
+  Interpolation2D::DataType xyz;
+  for (const auto &calibration : control_table.calibration()) {
+    xyz.push_back(std::make_tuple(calibration.speed(),
+                                  calibration.acceleration(),
+                                  calibration.command()));
+  }
+  control_interpolation_.reset(new Interpolation2D);
+  ACHECK(control_interpolation_->Init(xyz))
+      << "Fail to load control calibration table";
+}
+
+void MPCController::UpdateState(SimpleMPCDebug *debug) {
+  const auto &com = injector_->vehicle_state()->ComputeCOMPosition(lr_);
+  ComputeLateralErrors(com.x(), com.y(), injector_->vehicle_state()->heading(),
+                       injector_->vehicle_state()->linear_velocity(),
+                       injector_->vehicle_state()->angular_velocity(),
+                       injector_->vehicle_state()->linear_acceleration(),
+                       trajectory_analyzer_, debug);
+
+  // State matrix update;
+  matrix_state_(0, 0) = debug->lateral_error();
+  matrix_state_(1, 0) = debug->lateral_error_rate();
+  matrix_state_(2, 0) = debug->heading_error();
+  matrix_state_(3, 0) = debug->heading_error_rate();
+  matrix_state_(4, 0) = debug->station_error();
+  matrix_state_(5, 0) = debug->speed_error();
+}
+
+void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {
+  const double v = std::max(injector_->vehicle_state()->linear_velocity(),
+                            minimum_speed_protection_);
+  matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
+  matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
+  matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;
+  matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;
+
+  Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());
+  matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *
+               (matrix_i + ts_ * 0.5 * matrix_a_);
+
+  matrix_c_(1, 0) = (lr_ * cr_ - lf_ * cf_) / mass_ / v - v;
+  matrix_c_(3, 0) = -(lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_ / v;
+  matrix_cd_ = matrix_c_ * debug->ref_heading_rate() * ts_;
+}
+
+void MPCController::FeedforwardUpdate(SimpleMPCDebug *debug) {
+  const double v = injector_->vehicle_state()->linear_velocity();
+  const double kv =
+      lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;
+  steer_angle_feedforwardterm_ = Wheel2SteerPct(
+      wheelbase_ * debug->curvature() + kv * v * v * debug->curvature());
+}
+
+void MPCController::ComputeLateralErrors(
+    const double x, const double y, const double theta, const double linear_v,
+    const double angular_v, const double linear_a,
+    const TrajectoryAnalyzer &trajectory_analyzer, SimpleMPCDebug *debug) {
+  const auto matched_point =
+      trajectory_analyzer.QueryNearestPointByPosition(x, y);
+
+  const double dx = x - matched_point.path_point().x();
+  const double dy = y - matched_point.path_point().y();
+
+  const double cos_matched_theta = std::cos(matched_point.path_point().theta());
+  const double sin_matched_theta = std::sin(matched_point.path_point().theta());
+  // d_error = cos_matched_theta * dy - sin_matched_theta * dx;
+  debug->set_lateral_error(cos_matched_theta * dy - sin_matched_theta * dx);
+
+  // matched_theta = matched_point.path_point().theta();
+  debug->set_ref_heading(matched_point.path_point().theta());
+  const double delta_theta =
+      common::math::NormalizeAngle(theta - debug->ref_heading());
+  debug->set_heading_error(delta_theta);
+
+  const double sin_delta_theta = std::sin(delta_theta);
+  // d_error_dot = chassis_v * sin_delta_theta;
+  double lateral_error_dot = linear_v * sin_delta_theta;
+  double lateral_error_dot_dot = linear_a * sin_delta_theta;
+  if (FLAGS_reverse_heading_control) {
+    if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
+      lateral_error_dot = -lateral_error_dot;
+      lateral_error_dot_dot = -lateral_error_dot_dot;
+    }
+  }
+
+  debug->set_lateral_error_rate(lateral_error_dot);
+  debug->set_lateral_acceleration(lateral_error_dot_dot);
+  debug->set_lateral_jerk(
+      (debug->lateral_acceleration() - previous_lateral_acceleration_) / ts_);
+  previous_lateral_acceleration_ = debug->lateral_acceleration();
+
+  // matched_kappa = matched_point.path_point().kappa();
+  debug->set_curvature(matched_point.path_point().kappa());
+  // theta_error = delta_theta;
+  debug->set_heading_error(delta_theta);
+  // theta_error_dot = angular_v - matched_point.path_point().kappa() *
+  // matched_point.v();
+  debug->set_heading_rate(angular_v);
+  debug->set_ref_heading_rate(debug->curvature() * matched_point.v());
+  debug->set_heading_error_rate(debug->heading_rate() -
+                                debug->ref_heading_rate());
+
+  debug->set_heading_acceleration(
+      (debug->heading_rate() - previous_heading_rate_) / ts_);
+  debug->set_ref_heading_acceleration(
+      (debug->ref_heading_rate() - previous_ref_heading_rate_) / ts_);
+  debug->set_heading_error_acceleration(debug->heading_acceleration() -
+                                        debug->ref_heading_acceleration());
+  previous_heading_rate_ = debug->heading_rate();
+  previous_ref_heading_rate_ = debug->ref_heading_rate();
+
+  debug->set_heading_jerk(
+      (debug->heading_acceleration() - previous_heading_acceleration_) / ts_);
+  debug->set_ref_heading_jerk(
+      (debug->ref_heading_acceleration() - previous_ref_heading_acceleration_) /
+      ts_);
+  debug->set_heading_error_jerk(debug->heading_jerk() -
+                                debug->ref_heading_jerk());
+  previous_heading_acceleration_ = debug->heading_acceleration();
+  previous_ref_heading_acceleration_ = debug->ref_heading_acceleration();
+}
+
+void MPCController::ComputeLongitudinalErrors(
+    const TrajectoryAnalyzer *trajectory_analyzer, SimpleMPCDebug *debug) {
+  // the decomposed vehicle motion onto Frenet frame
+  // s: longitudinal accumulated distance along reference trajectory
+  // s_dot: longitudinal velocity along reference trajectory
+  // d: lateral distance w.r.t. reference trajectory
+  // d_dot: lateral distance change rate, i.e. dd/dt
+  double s_matched = 0.0;
+  double s_dot_matched = 0.0;
+  double d_matched = 0.0;
+  double d_dot_matched = 0.0;
+
+  const auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(
+      injector_->vehicle_state()->x(), injector_->vehicle_state()->y());
+
+  trajectory_analyzer->ToTrajectoryFrame(
+      injector_->vehicle_state()->x(), injector_->vehicle_state()->y(),
+      injector_->vehicle_state()->heading(),
+      injector_->vehicle_state()->linear_velocity(), matched_point, &s_matched,
+      &s_dot_matched, &d_matched, &d_dot_matched);
+
+#ifndef ADCTEST
+  const double current_control_time =  Clock::NowInSeconds();
+#else
+  const double current_control_time =  0;
+#endif
+
+  TrajectoryPoint reference_point =
+      trajectory_analyzer->QueryNearestPointByAbsoluteTime(
+          current_control_time);
+
+  ADEBUG << "matched point:" << matched_point.DebugString();
+  ADEBUG << "reference point:" << reference_point.DebugString();
+
+  const double linear_v = injector_->vehicle_state()->linear_velocity();
+  const double linear_a = injector_->vehicle_state()->linear_acceleration();
+  double heading_error = common::math::NormalizeAngle(
+      injector_->vehicle_state()->heading() - matched_point.theta());
+  double lon_speed = linear_v * std::cos(heading_error);
+  double lon_acceleration = linear_a * std::cos(heading_error);
+  double one_minus_kappa_lat_error = 1 - reference_point.path_point().kappa() *
+                                             linear_v * std::sin(heading_error);
+
+  debug->set_station_reference(reference_point.path_point().s());
+  debug->set_station_feedback(s_matched);
+  debug->set_station_error(reference_point.path_point().s() - s_matched);
+  debug->set_speed_reference(reference_point.v());
+  debug->set_speed_feedback(lon_speed);
+  debug->set_speed_error(reference_point.v() - s_dot_matched);
+  debug->set_acceleration_reference(reference_point.a());
+  debug->set_acceleration_feedback(lon_acceleration);
+  debug->set_acceleration_error(reference_point.a() -
+                                lon_acceleration / one_minus_kappa_lat_error);
+  double jerk_reference =
+      (debug->acceleration_reference() - previous_acceleration_reference_) /
+      ts_;
+  double lon_jerk =
+      (debug->acceleration_feedback() - previous_acceleration_) / ts_;
+  debug->set_jerk_reference(jerk_reference);
+  debug->set_jerk_feedback(lon_jerk);
+  debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
+  previous_acceleration_reference_ = debug->acceleration_reference();
+  previous_acceleration_ = debug->acceleration_feedback();
+}
+
+}  // namespace control
+}  // namespace apollo

src/decition/adcapollompc/adcapollompc.pro

@@ -0,0 +1,143 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+DEFINES += ADCTEST
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        ../../../thirdpartylib/apollo/cyber/binary.cc \
+        ../../../thirdpartylib/apollo/cyber/common/file.cc \
+        ../../../thirdpartylib/apollo/cyber/common/global_data.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/choreography_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/classic_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/cyber_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/perf_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/run_mode_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/scheduler_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/transport_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/time/clock.cc \
+        ../../../thirdpartylib/apollo/cyber/time/duration.cc \
+        ../../../thirdpartylib/apollo/cyber/time/time.cc \
+        ../../../thirdpartylib/apollo/modules/canbus/proto/chassis.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/configs/config_gflags.cc \
+        ../../../thirdpartylib/apollo/modules/common/configs/proto/vehicle_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/configs/vehicle_config_helper.cc \
+        ../../../thirdpartylib/apollo/modules/common/filters/digital_filter.cc \
+        ../../../thirdpartylib/apollo/modules/common/filters/digital_filter_coefficients.cc \
+        ../../../thirdpartylib/apollo/modules/common/filters/mean_filter.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/aabox2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/box2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/line_segment2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/linear_interpolation.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/math_utils.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/mpc_osqp.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/polygon2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/search.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/vec2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/drive_state.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/error_code.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/geometry.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/header.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/pnc_point.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/vehicle_signal.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/vehicle_state/proto/vehicle_state.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/vehicle_state/vehicle_state_provider.cc \
+        ../../../thirdpartylib/apollo/modules/control/common/control_gflags.cc \
+        ../../../thirdpartylib/apollo/modules/control/common/interpolation_1d.cc \
+        ../../../thirdpartylib/apollo/modules/control/common/interpolation_2d.cc \
+        ../../../thirdpartylib/apollo/modules/control/common/trajectory_analyzer.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/calibration_table.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/control_cmd.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/control_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/gain_scheduler_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/input_debug.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/lat_controller_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/leadlag_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/lon_controller_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/mpc_controller_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/mrac_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/pad_msg.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/pid_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/dreamview/proto/chart.pb.cc \
+        ../../../thirdpartylib/apollo/modules/localization/common/localization_gflags.cc \
+        ../../../thirdpartylib/apollo/modules/localization/proto/localization.pb.cc \
+        ../../../thirdpartylib/apollo/modules/localization/proto/localization_status.pb.cc \
+        ../../../thirdpartylib/apollo/modules/localization/proto/pose.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_clear_area.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_crosswalk.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_geometry.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_id.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_junction.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_lane.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_overlap.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_parking_space.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_pnc_junction.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_road.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_rsu.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_signal.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_speed_bump.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_stop_sign.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_yield_sign.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/relative_map/proto/navigation.pb.cc \
+        ../../../thirdpartylib/apollo/modules/perception/proto/perception_obstacle.pb.cc \
+        ../../../thirdpartylib/apollo/modules/perception/proto/traffic_light_detection.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/decision.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/math/fem_pos_deviation_smoother_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/open_space_task_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/planner_open_space_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/planning.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/planning_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/planning_internal.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/sl_boundary.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/task_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/routing/proto/routing.pb.cc \
+        adc_mpc_controller.cc \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    ../../../thirdpartylib/apollo/modules/canbus/proto/chassis.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/configs/proto/vehicle_config.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/drive_state.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/error_code.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/geometry.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/header.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/pnc_point.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/vehicle_signal.pb.h \
+    ../../../thirdpartylib/apollo/modules/control/controller/mpc_controller.h \
+    ../../../thirdpartylib/apollo/modules/map/proto/map_rsu.pb.h \
+    ../../../thirdpartylib/apollo/modules/planning/proto/planning.pb.h
+
+
+INCLUDEPATH += /usr/include/eigen3
+INCLUDEPATH += $$PWD/../../../thirdpartylib/apollo
+
+INCLUDEPATH += ../../..//thirdpartylib/osqp-0.5.0/include
+INCLUDEPATH += ../../../thirdpartylib/abseil-cpp
+
+LIBS += -lprotobuf -lglog -lgflags -lgtest
+
+
+LIBS += -L$$PWD/../../../thirdpartylib/grpc/lib
+LIBS +=  -labsl_raw_hash_set -labsl_hashtablez_sampler -labsl_exponential_biased -labsl_hash -labsl_bad_variant_access -labsl_city -labsl_status -labsl_cord -labsl_str_format_internal -labsl_synchronization -labsl_graphcycles_internal -labsl_symbolize -labsl_demangle_internal -labsl_stacktrace -labsl_debugging_internal -labsl_malloc_internal -labsl_time -labsl_time_zone -labsl_civil_time -labsl_strings -labsl_strings_internal -labsl_throw_delegate -labsl_int128 -labsl_base -labsl_spinlock_wait -labsl_bad_optional_access -labsl_raw_logging_internal -labsl_log_severity
+
+LIBS += -L$$PWD/../../../thirdpartylib/osqp-0.5.0/build/out
+
+LIBS += -losqp

src/decition/adcapollompc/apolloproto.sh

@@ -0,0 +1,13 @@
+protoc -I=. --cpp_out=.  cyber/proto/*.proto
+protoc -I=. --cpp_out=.  modules/canbus/proto/*.proto
+protoc -I=. --cpp_out=.  modules/common/proto/*.proto
+protoc -I=. --cpp_out=.  modules/common/vehicle_state/proto/*.proto
+protoc -I=. --cpp_out=.  modules/control/proto/*.proto
+protoc -I=. --cpp_out=.  modules/dreamview/proto/*.proto
+protoc -I=. --cpp_out=.  modules/localization/proto/*.proto
+protoc -I=. --cpp_out=.  modules/map/proto/*.proto
+protoc -I=. --cpp_out=.  modules/perception/proto/*.proto
+protoc -I=. --cpp_out=.  modules/planning/proto/*.proto
+protoc -I=. --cpp_out=.  modules/routing/proto/*.proto
+protoc -I=. --cpp_out=.  modules/common/configs/proto/*.proto
+protoc -I=. --cpp_out=.  modules/common/proto/*.proto

src/decition/adcapollompc/main.cpp

@@ -0,0 +1,208 @@
+#include <QCoreApplication>
+
+#include "modules/control/controller/mpc_controller.h"
+
+#include "cyber/common/file.h"
+#include "cyber/common/log.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "cyber/time/clock.h"
+#include "modules/common/vehicle_state/vehicle_state_provider.h"
+#include "modules/control/common/control_gflags.h"
+#include "modules/control/proto/control_conf.pb.h"
+#include "modules/localization/common/localization_gflags.h"
+#include "modules/planning/proto/planning.pb.h"
+
+#include "modules/common/configs/config_gflags.h"
+
+namespace apollo {
+namespace control {
+
+using apollo::cyber::Clock;
+using PlanningTrajectoryPb = planning::ADCTrajectory;
+using LocalizationPb = localization::LocalizationEstimate;
+using ChassisPb = canbus::Chassis;
+using apollo::common::VehicleStateProvider;
+
+class MPCControllerTest : public ::testing::Test, MPCController {
+ public:
+  virtual void SetupTestCase() {
+    FLAGS_v = 4;
+    FLAGS_control_conf_file =
+        "/apollo/modules//control/testdata/mpc_controller_test/"
+        "control_conf.pb.txt ";
+    ControlConf control_conf;
+    ACHECK(cyber::common::GetProtoFromFile(FLAGS_control_conf_file,
+                                           &control_conf));
+    mpc_conf_ = control_conf.mpc_controller_conf();
+
+    timestamp_ = Clock::NowInSeconds();
+  }
+
+  void ComputeLateralErrors(const double x, const double y, const double theta,
+                            const double linear_v, const double angular_v,
+                            const double linear_a,
+                            const TrajectoryAnalyzer &trajectory_analyzer,
+                            SimpleMPCDebug *debug) {
+    MPCController::ComputeLateralErrors(x, y, theta, linear_v, angular_v,
+                                        linear_a, trajectory_analyzer, debug);
+  }
+
+ protected:
+  LocalizationPb LoadLocalizaionPb(const std::string &filename) {
+    LocalizationPb localization_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &localization_pb))
+        << "Failed to open file " << filename;
+    localization_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return localization_pb;
+  }
+
+  ChassisPb LoadChassisPb(const std::string &filename) {
+    ChassisPb chassis_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &chassis_pb))
+        << "Failed to open file " << filename;
+    chassis_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return chassis_pb;
+  }
+
+  PlanningTrajectoryPb LoadPlanningTrajectoryPb(const std::string &filename) {
+    PlanningTrajectoryPb planning_trajectory_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &planning_trajectory_pb))
+        << "Failed to open file " << filename;
+    planning_trajectory_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return planning_trajectory_pb;
+  }
+
+  MPCControllerConf mpc_conf_;
+
+  double timestamp_ = 0.0;
+};
+
+TEST_F(MPCControllerTest, ComputeLateralErrors) {
+  auto localization_pb = LoadLocalizaionPb(
+      "/home/yuchuli/File/git/apollo/modules//control/testdata/mpc_controller_test/"
+      "1_localization.pb.txt");
+  auto chassis_pb = LoadChassisPb(
+      "/home/yuchuli/File/git/apollo/modules/control/testdata/mpc_controller_test/1_chassis.pb.txt");
+  FLAGS_enable_map_reference_unify = false;
+  auto injector = std::make_shared<DependencyInjector>();
+  auto vehicle_state = injector->vehicle_state();
+  vehicle_state->Update(localization_pb, chassis_pb);
+
+  auto planning_trajectory_pb = LoadPlanningTrajectoryPb(
+      "/home/yuchuli/File/git/apollo/modules//control/testdata/mpc_controller_test/"
+      "1_planning.pb.txt");
+  TrajectoryAnalyzer trajectory_analyzer(&planning_trajectory_pb);
+
+  ControlCommand cmd;
+  SimpleMPCDebug *debug = cmd.mutable_debug()->mutable_simple_mpc_debug();
+
+  ComputeLateralErrors(
+      vehicle_state->x(), vehicle_state->y(), vehicle_state->heading(),
+      vehicle_state->linear_velocity(), vehicle_state->angular_velocity(),
+      vehicle_state->linear_acceleration(), trajectory_analyzer, debug);
+
+  double theta_error_expected = -0.03549;
+  double theta_error_dot_expected = 0.0044552856731;
+  double d_error_expected = 1.30917375441;
+  double d_error_dot_expected = 0.0;
+  double matched_theta_expected = -1.81266;
+  double matched_kappa_expected = -0.00237307;
+
+  EXPECT_NEAR(debug->heading_error(), theta_error_expected, 0.001);
+  EXPECT_NEAR(debug->heading_error_rate(), theta_error_dot_expected, 0.001);
+  EXPECT_NEAR(debug->lateral_error(), d_error_expected, 0.001);
+  EXPECT_NEAR(debug->lateral_error_rate(), d_error_dot_expected, 0.001);
+  EXPECT_NEAR(debug->ref_heading(), matched_theta_expected, 0.001);
+  EXPECT_NEAR(debug->curvature(), matched_kappa_expected, 0.001);
+}
+
+}  // namespace control
+}  // namespace apollo
+
+
+//using apollo::cyber::Clock;
+//using PlanningTrajectoryPb = planning::ADCTrajectory;
+//using LocalizationPb = localization::LocalizationEstimate;
+//using ChassisPb = canbus::Chassis;
+//using apollo::common::VehicleStateProvider;
+
+double timestamp_ = 0.0;
+
+apollo::localization::LocalizationEstimate LoadLocalizaionPb(const std::string &filename) {
+  apollo::localization::LocalizationEstimate localization_pb;
+  ACHECK(apollo::cyber::common::GetProtoFromFile(filename, &localization_pb))
+      << "Failed to open file " << filename;
+  localization_pb.mutable_header()->set_timestamp_sec(timestamp_);
+  return localization_pb;
+}
+
+apollo::canbus::Chassis LoadChassisPb(const std::string &filename) {
+  apollo::canbus::Chassis chassis_pb;
+  ACHECK(apollo::cyber::common::GetProtoFromFile(filename, &chassis_pb))
+      << "Failed to open file " << filename;
+  chassis_pb.mutable_header()->set_timestamp_sec(timestamp_);
+  return chassis_pb;
+}
+
+
+
+apollo::planning::ADCTrajectory LoadPlanningTrajectoryPb(const std::string &filename) {
+  apollo::planning::ADCTrajectory planning_trajectory_pb;
+  ACHECK(apollo::cyber::common::GetProtoFromFile(filename, &planning_trajectory_pb))
+      << "Failed to open file " << filename;
+  planning_trajectory_pb.mutable_header()->set_timestamp_sec(timestamp_);
+  return planning_trajectory_pb;
+}
+
+void mpctest()
+{
+
+    auto localization_pb = LoadLocalizaionPb(
+        "/home/yuchuli/File/git/apollo/modules//control/testdata/mpc_controller_test/"
+        "1_localization.pb.txt");
+    auto chassis_pb = LoadChassisPb(
+        "/home/yuchuli/File/git/apollo/modules/control/testdata/mpc_controller_test/1_chassis.pb.txt");
+    FLAGS_enable_map_reference_unify = false;
+    FLAGS_vehicle_config_path  = "/home/yuchuli/qt/modularization/thirdpartylib/apollo/modules/common/data/vehicle_param.pb.txt";
+    auto injector = std::make_shared<apollo::control::DependencyInjector>();
+    auto vehicle_state = injector->vehicle_state();
+    vehicle_state->Update(localization_pb, chassis_pb);
+
+    auto planning_trajectory_pb = LoadPlanningTrajectoryPb(
+        "/home/yuchuli/File/git/apollo/modules//control/testdata/mpc_controller_test/"
+        "1_planning.pb.txt");
+    apollo::control::TrajectoryAnalyzer trajectory_analyzer(&planning_trajectory_pb);
+
+    apollo::control::ControlCommand cmd;
+
+    apollo::control::ControlConf  xConf;
+
+    std::string confilename = "/home/yuchuli/qt/modularization/thirdpartylib/apollo/modules/control/testdata/mpc_controller_test/control_conf.pb.txt";
+    ACHECK(apollo::cyber::common::GetProtoFromFile(confilename, &xConf))
+        << "Failed to open file " << confilename;
+
+    apollo::control::MPCController xController;
+    xController.Init(injector,&xConf);
+
+    xController.ComputeControlCommand(&localization_pb,&chassis_pb,&planning_trajectory_pb,&cmd);
+
+    int a = 1;
+    a= 2;
+
+
+
+}
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    testing::InitGoogleTest(&argc,argv);
+
+    mpctest();
+
+//    RUN_ALL_TESTS();
+
+    return a.exec();
+}

src/decition/adcapollompc/mpc_controller_test.cc

@@ -0,0 +1,134 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * 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 "modules/control/controller/mpc_controller.h"
+
+#include "cyber/common/file.h"
+#include "cyber/common/log.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "cyber/time/clock.h"
+#include "modules/common/vehicle_state/vehicle_state_provider.h"
+#include "modules/control/common/control_gflags.h"
+#include "modules/control/proto/control_conf.pb.h"
+#include "modules/localization/common/localization_gflags.h"
+#include "modules/planning/proto/planning.pb.h"
+
+namespace apollo {
+namespace control {
+
+using apollo::cyber::Clock;
+using PlanningTrajectoryPb = planning::ADCTrajectory;
+using LocalizationPb = localization::LocalizationEstimate;
+using ChassisPb = canbus::Chassis;
+using apollo::common::VehicleStateProvider;
+
+class MPCControllerTest : public ::testing::Test, MPCController {
+ public:
+  virtual void SetupTestCase() {
+    FLAGS_v = 4;
+    FLAGS_control_conf_file =
+        "/apollo/modules//control/testdata/mpc_controller_test/"
+        "control_conf.pb.txt ";
+    ControlConf control_conf;
+    ACHECK(cyber::common::GetProtoFromFile(FLAGS_control_conf_file,
+                                           &control_conf));
+    mpc_conf_ = control_conf.mpc_controller_conf();
+
+    timestamp_ = Clock::NowInSeconds();
+  }
+
+  void ComputeLateralErrors(const double x, const double y, const double theta,
+                            const double linear_v, const double angular_v,
+                            const double linear_a,
+                            const TrajectoryAnalyzer &trajectory_analyzer,
+                            SimpleMPCDebug *debug) {
+    MPCController::ComputeLateralErrors(x, y, theta, linear_v, angular_v,
+                                        linear_a, trajectory_analyzer, debug);
+  }
+
+ protected:
+  LocalizationPb LoadLocalizaionPb(const std::string &filename) {
+    LocalizationPb localization_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &localization_pb))
+        << "Failed to open file " << filename;
+    localization_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return localization_pb;
+  }
+
+  ChassisPb LoadChassisPb(const std::string &filename) {
+    ChassisPb chassis_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &chassis_pb))
+        << "Failed to open file " << filename;
+    chassis_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return chassis_pb;
+  }
+
+  PlanningTrajectoryPb LoadPlanningTrajectoryPb(const std::string &filename) {
+    PlanningTrajectoryPb planning_trajectory_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &planning_trajectory_pb))
+        << "Failed to open file " << filename;
+    planning_trajectory_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return planning_trajectory_pb;
+  }
+
+  MPCControllerConf mpc_conf_;
+
+  double timestamp_ = 0.0;
+};
+
+TEST_F(MPCControllerTest, ComputeLateralErrors) {
+  auto localization_pb = LoadLocalizaionPb(
+      "/apollo/modules//control/testdata/mpc_controller_test/"
+      "1_localization.pb.txt");
+  auto chassis_pb = LoadChassisPb(
+      "/apollo/modules/control/testdata/mpc_controller_test/1_chassis.pb.txt");
+  FLAGS_enable_map_reference_unify = false;
+  auto injector = std::make_shared<DependencyInjector>();
+  auto vehicle_state = injector->vehicle_state();
+  vehicle_state->Update(localization_pb, chassis_pb);
+
+  auto planning_trajectory_pb = LoadPlanningTrajectoryPb(
+      "/apollo/modules//control/testdata/mpc_controller_test/"
+      "1_planning.pb.txt");
+  TrajectoryAnalyzer trajectory_analyzer(&planning_trajectory_pb);
+
+  ControlCommand cmd;
+  SimpleMPCDebug *debug = cmd.mutable_debug()->mutable_simple_mpc_debug();
+
+  ComputeLateralErrors(
+      vehicle_state->x(), vehicle_state->y(), vehicle_state->heading(),
+      vehicle_state->linear_velocity(), vehicle_state->angular_velocity(),
+      vehicle_state->linear_acceleration(), trajectory_analyzer, debug);
+
+  double theta_error_expected = -0.03549;
+  double theta_error_dot_expected = 0.0044552856731;
+  double d_error_expected = 1.30917375441;
+  double d_error_dot_expected = 0.0;
+  double matched_theta_expected = -1.81266;
+  double matched_kappa_expected = -0.00237307;
+
+  EXPECT_NEAR(debug->heading_error(), theta_error_expected, 0.001);
+  EXPECT_NEAR(debug->heading_error_rate(), theta_error_dot_expected, 0.001);
+  EXPECT_NEAR(debug->lateral_error(), d_error_expected, 0.001);
+  EXPECT_NEAR(debug->lateral_error_rate(), d_error_dot_expected, 0.001);
+  EXPECT_NEAR(debug->ref_heading(), matched_theta_expected, 0.001);
+  EXPECT_NEAR(debug->curvature(), matched_kappa_expected, 0.001);
+}
+
+}  // namespace control
+}  // namespace apollo