Autoware的MPC源码解析（二）mpc_follower解析：接口解析

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

在Autoware的mpc源码中包含了两个节点，一个是/mpc_waypoints_converter，另一个是/mpc_follower。关系如下图所示。
此图来源于源码

关于/mpc_waypoints_converter节点的解析可参考此博文：https://blog.csdn.net/cyj1871/article/details/98490799

本文开始介绍/mpc_follower节点，本文主要解析此节点的接口函数。此节点主要是接收/mpc_waypoints_converter节点发布的路径信息后，经过计算后发布横纵向目标控制指令。发布两个topic分别是：

//发布目标速度指令：目标纵向速度，目标横摆角速度
pub_twist_cmd_ = nh_.advertise(out_twist, 1);
void MPCFollower::publishTwist(const double &vel_cmd, const double &omega_cmd)
{
  /* convert steering to twist */
  geometry_msgs::TwistStamped twist;
  twist.header.frame_id = "/base_link";
  twist.header.stamp = ros::Time::now();
  twist.twist.linear.x = vel_cmd;					//目标纵向速度
  twist.twist.linear.y = 0.0;
  twist.twist.linear.z = 0.0;
  twist.twist.angular.x = 0.0;
  twist.twist.angular.y = 0.0;
  twist.twist.angular.z = omega_cmd;				//目标横摆角速度
  pub_twist_cmd_.publish(twist);
}

// 发布目标控制指令：目标转向角，目标纵向速度，目标加速度
pub_steer_vel_ctrl_cmd_ = nh_.advertise(out_vehicle_cmd, 1);
void MPCFollower::publishCtrlCmd(const double &vel_cmd, const double &acc_cmd, const double &steer_cmd)
{
  autoware_msgs::ControlCommandStamped cmd;
  cmd.header.frame_id = "/base_link";
  cmd.header.stamp = ros::Time::now();
  cmd.cmd.linear_velocity = vel_cmd;				//目标纵向速度
  cmd.cmd.linear_acceleration = acc_cmd;			//目标加速度
  cmd.cmd.steering_angle = steer_cmd;				//目标转向角
  pub_steer_vel_ctrl_cmd_.publish(cmd);
}

这两个topic可通过修改以下output_interface_参数来修改成只发布某一个topic或两个都发布：

pnh_.param("output_interface", output_interface_, std::string("all"));	

此节点主要订阅三个topic（此节点也订阅和发布了debug相关的topic，目前不做解析）分别是：

//接受车辆当前位姿信息：pose
sub_pose_ = nh_.subscribe(in_selfpose, 1, &MPCFollower::callbackPose, this);
void MPCFollower::callbackPose(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  vehicle_status_.header = msg->header;
  vehicle_status_.pose = msg->pose;					//车辆pose信息
  my_position_ok_ = true;							//pose信息接受成功标志位
};

//接受车辆状态信息：当前车速，当前转向角
sub_vehicle_status_ = nh_.subscribe(in_vehicle_status, 1, &MPCFollower::callbackVehicleStatus, this);
void MPCFollower::callbackVehicleStatus(const autoware_msgs::VehicleStatus &msg)
{
  vehicle_status_.tire_angle_rad = msg.angle;							//当前转向角
  vehicle_status_.twist.linear.x = amathutils::kmph2mps(msg.speed);		//当前车速
  my_steering_ok_ = true;												//转向角信息接收成功标志位
  my_velocity_ok_ = true;												//速度信息接收成功标志位
};

//接受/mpc_waypoints_converter节点发布的局部路径信息
sub_ref_path_ = nh_.subscribe(in_waypoints, 1, &MPCFollower::callbackRefPath, this);

接受到局部路径信息后，先是计算路径点的相对时间：

/* calculate relative time */
std::vector relative_time;
MPCUtils::calcPathRelativeTime(current_waypoints_, relative_time);
//计算相对时间，设置路径起点的时间为0，之后根据每个路径点的目标速度以及点与点之间的距离计算出车辆到达每个路径点时相对于起点的时间

之后对路径进行重置：

/* resampling */
MPCUtils::convertWaypointsToMPCTrajWithDistanceResample(current_waypoints_, relative_time, traj_resample_dist_, traj);
//按照设置的均分距离traj_resample_dist_使用线性插值法对路径进行均分重置
  
MPCUtils::convertEulerAngleToMonotonic(traj.yaw);
//此函数是对路径点的航向角进行修改，保证相邻路径点的航向角的差值处于-pi～pi之间

对重置的路径进行光滑处理（可通过修改enable_path_smoothing_变量的值选择是否执行此处理）：

 /* path smoothing */
  if (enable_path_smoothing_)
  {
    for (int i = 0; i < path_smoothing_times_; ++i)
    {
      if (!MoveAverageFilter::filt_vector(path_filter_moving_ave_num_, traj.x) ||  		//使用的方法是移动平均滤波器
          !MoveAverageFilter::filt_vector(path_filter_moving_ave_num_, traj.y) ||
          !MoveAverageFilter::filt_vector(path_filter_moving_ave_num_, traj.yaw) ||
          !MoveAverageFilter::filt_vector(path_filter_moving_ave_num_, traj.vx))
      {
        ROS_WARN("[MPC] path callback: filtering error. stop filtering");
        return;
      }
    }
  }

重新求解航向角（可通过修改enable_yaw_recalculation_变量的值选择是否执行此操作）：

  /* calculate yaw angle */
  if (enable_yaw_recalculation_)
  {
    MPCUtils::calcTrajectoryYawFromXY(traj);			//重新求解路径点的航向角
    MPCUtils::convertEulerAngleToMonotonic(traj.yaw);
  }

计算每个路径点的曲率：

  /* calculate curvature /
  MPCUtils::calcTrajectoryCurvature(traj, curvature_smoothing_num_);
  const double max_k = *max_element(traj.k.begin(), traj.k.end());					//找最大的曲率
  const double min_k = *min_element(traj.k.begin(), traj.k.end());					//找最小的曲率
  DEBUG_INFO("[MPC] path callback: trajectory curvature : max_k = %f, min_k = %f", max_k, min_k);

设置路径的最后一个路径点的目标速度为0

  /* add end point with vel=0 on traj for mpc prediction */
  const double mpc_predict_time_length = (mpc_param_.prediction_horizon + 1) * mpc_param_.prediction_sampling_time;
  //计算mpc的预测时长
  
  const double end_velocity = 0.0;
  traj.vx.back() = end_velocity;			//设置最后一个点的目标速度为0
  traj.push_back(traj.x.back(), traj.y.back(), traj.z.back(), traj.yaw.back(),
                 end_velocity, traj.k.back(), traj.relative_time.back() + mpc_predict_time_length);

发布路径信息用于观察：

  /* publish trajectory for visualize */
  visualization_msgs::Marker markers;
  convertTrajToMarker(ref_traj_, markers, "ref_traj", 0.0, 0.5, 1.0, 0.05);
  pub_debug_filtered_traj_.publish(markers);