Autoware的MPC源码解析（四）mpc_follower解析：主循环程序解析

版权声明：本文为博主原创文章，未经博主允许不得转载。
源码地址：https://gitlab.com/autowarefoundation/autoware.ai/autoware/wikis/Source-Build
如有错误或建议，欢迎提出。

本文对mpc_follower的主循环程序进行解析。

1. MPCFollower::MPCFollower()解析

在程序启动后，首先会执行MPCFollower::MPCFollower()构造函数，主要进行如下内容：
（1）参数赋值

pnh_.param("show_debug_info", show_debug_info_, bool(false));
pnh_.param("publish_debug_values", publish_debug_values_, bool(true));
pnh_.param("ctrl_period", ctrl_period_, double(0.03));//控制周期
pnh_.param("enable_path_smoothing", enable_path_smoothing_, bool(true));
 ......

（2）车辆模型构建（车辆模型的介绍可参考Autoware的MPC源码解析（三）mpc_follower解析：车辆模型介绍）

  /* vehicle model setup */
  pnh_.param("vehicle_model_type", vehicle_model_type_, std::string("kinematics"));//
  if (vehicle_model_type_ == "kinematics")					//车辆运动学误差模型
  {
    double steer_tau;
    pnh_.param("vehicle_model_steer_tau", steer_tau, double(0.1));

    vehicle_model_ptr_ = std::make_shared(wheelbase_, amathutils::deg2rad(steer_lim_deg_), steer_tau);
    ROS_INFO("[MPC] set vehicle_model = kinematics");
  }
  else if (vehicle_model_type_ == "kinematics_no_delay")	//车辆运动学误差模型 no delay
  {
    vehicle_model_ptr_ = std::make_shared(wheelbase_, amathutils::deg2rad(steer_lim_deg_));
    ROS_INFO("[MPC] set vehicle_model = kinematics_no_delay");
  }
  else if (vehicle_model_type_ == "dynamics")				//车辆动力学误差模型
  {
    double mass_fl, mass_fr, mass_rl, mass_rr, cf, cr;
    pnh_.param("mass_fl", mass_fl, double(600));
    pnh_.param("mass_fr", mass_fr, double(600));
    pnh_.param("mass_rl", mass_rl, double(600));
    pnh_.param("mass_rr", mass_rr, double(600));
    pnh_.param("cf", cf, double(155494.663));
    pnh_.param("cr", cr, double(155494.663));

    vehicle_model_ptr_ = std::make_shared(wheelbase_, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr);
    ROS_INFO("[MPC] set vehicle_model = dynamics");
  }
  else
  {
    ROS_ERROR("[MPC] vehicle_model_type is undefined");
  }

（3）选择QP优化方法

/* QP solver setup */
  std::string qp_solver_type_;
  pnh_.param("qp_solver_type", qp_solver_type_, std::string("unconstraint_fast"));
  if (qp_solver_type_ == "unconstraint")
  {
    qpsolver_ptr_ = std::make_shared();
    ROS_INFO("[MPC] set qp solver = unconstraint");
  }
  else if (qp_solver_type_ == "unconstraint_fast")
  {
    qpsolver_ptr_ = std::make_shared();
    ROS_INFO("[MPC] set qp solver = unconstraint_fast");
  }
  // else if (qp_solver_type_ == "qpoases_hotstart")
  // {
  //   int max_iter = 200;
  //   qpsolver_ptr_ = std::make_shared(max_iter);
  //   ROS_INFO("[MPC] set qp solver = qpoases_hotstart");
  // }
  else
  {
    ROS_ERROR("[MPC] qp_solver_type is undefined");
  }

2. MPCFollower::timerCallback(const ros::TimerEvent &te)解析

timerCallback()函数是mpc的循环主程序，内容如下：
如果模型和优化方法为空，则发送制动指令：

if (vehicle_model_ptr_ == nullptr || qpsolver_ptr_ == nullptr)
  {
    DEBUG_INFO("[MPC] vehicle_model = %d, qp_solver = %d", !(vehicle_model_ptr_ == nullptr), !(qpsolver_ptr_ == nullptr));
    publishControlCommands(0.0, 0.0, steer_cmd_prev_, 0.0); // publish brake
    return;
  }

如果接受到的路径信息为空，且接收不到车辆的位姿信息，则发送制动指令：

if (ref_traj_.size() == 0 || !my_position_ok_ || !my_velocity_ok_ || !my_steering_ok_)
  {
    DEBUG_INFO("[MPC] MPC is not solved. ref_traj_.size() = %d, my_position_ok_ = %d,  my_velocity_ok_ = %d,  my_steering_ok_ = %d",
               ref_traj_.size(), my_position_ok_, my_velocity_ok_, my_steering_ok_);
    publishControlCommands(0.0, 0.0, steer_cmd_prev_, 0.0); // publish brake
    return;
  }

控制命令初始化：

  /* control command  */
  double vel_cmd = 0.0;
  double acc_cmd = 0.0;
  double steer_cmd = 0.0;
  double steer_vel_cmd = 0.0;

mpc求解并发布计算结果：

 /* solve MPC */
  auto start = std::chrono::system_clock::now();
  const bool mpc_solved = calculateMPC(vel_cmd, acc_cmd, steer_cmd, steer_vel_cmd);
  double elapsed_ms = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count() * 1.0e-6;
  DEBUG_INFO("[MPC] timerCallback: MPC calculating time = %f [ms]\n", elapsed_ms);

  /* publish computing time 发布mpc计算时间*/
  std_msgs::Float32 mpc_calc_time_msg;
  mpc_calc_time_msg.data = elapsed_ms;
  pub_debug_mpc_calc_time_.publish(mpc_calc_time_msg);

  if (!mpc_solved)//如果mpc没有解出结果，则指令置0
  {
    ROS_WARN("[MPC] MPC is not solved. publish 0 velocity.");
    vel_cmd = 0.0;
    acc_cmd = 0.0;
    steer_cmd = steer_cmd_prev_;
    steer_vel_cmd = 0.0;
  }

  publishControlCommands(vel_cmd, acc_cmd, steer_cmd, steer_vel_cmd);//发布指令