自动驾驶代码注解

1.从主函数开始看起： 

int main(int argc, char** argv) {
  ros::init(argc, argv, "control_pub"); // 初始化ros节点，节点名称为control_pub.
  ros::NodeHandle nh;                   // 实例化节点句柄，用于管理ros的相关API
  ROS_INFO("init !");

2.实例化一个订阅者sub：sub订阅的是车辆的实时信息，包括了一些坐标的变换。最后将车辆当前的状态信息存储在结构体vehicle_state_中，包括了车辆的x坐标、y坐标，x方向速度、y方向速度、车辆的合速度，以及车辆朝向heading。

  ros::Subscriber sub = nh.subscribe("/odom", 10, odomCallback);

3.实例化一个发布者control_pub：用于发布车辆的控制信息，消息类型为lgsvl_msgs::VehicleControlData，话题名为/vehicle_cmd

  ros::Publisher control_pub =
      nh.advertise<lgsvl_msgs::VehicleControlData>("/vehicle_cmd", 1000);

4.定义一个控制指令control_cmd（这里还没搞清楚）

  lgsvl_msgs::VehicleControlData control_cmd;
  control_cmd.header.stamp = ros::Time::now();
  control_cmd.target_gear = lgsvl_msgs::VehicleControlData::GEAR_DRIVE;
  control_cmd.target_wheel_angle = 0.0;

5.读取参考路径：将参考路径SingleLaneRoad.txt中的信息读取到xy_points当中，路径信息由大量的（x,y）坐标组成。

  // 读取参考线路径
  std::ifstream infile;
  infile.open(
      "src/vehicle_control/data/SingleLaneRoad.txt");  //将文件流对象与文件连接起来
  assert(infile.is_open());  //若失败,则输出错误消息,并终止程序运行

  std::vector<std::pair<double, double>> xy_points;
  std::string s;
  std::string x;
  std::string y;
  while (getline(infile, s)) {
    std::stringstream word(s);
    word >> x;
    word >> y;
    double pt_x = std::atof(x.c_str());
    double pt_y = std::atof(y.c_str());
    xy_points.push_back(std::make_pair(pt_x, pt_y));
  }
  infile.close();

6.计算路径点上的车辆朝向累积里程、曲率变化等（不确定）

 // Construct the reference_line path profile
  std::vector<double> headings;
  std::vector<double> accumulated_s;
  std::vector<double> kappas;
  std::vector<double> dkappas;
  std::unique_ptr<shenlan::control::ReferenceLine> reference_line =
      std::make_unique<shenlan::control::ReferenceLine>(xy_points);
  reference_line->ComputePathProfile(&headings, &accumulated_s, &kappas,
                                     &dkappas);

7.打印信息

  for (size_t i = 0; i < headings.size(); i++) {
    std::cout << "pt " << i << " heading: " << headings[i]
              << " acc_s: " << accumulated_s[i] << " kappa: " << kappas[i]
              << " dkappas: " << dkappas[i] << std::endl;
  }

8.将参考轨迹信息和根据参考轨迹求得的信息保存到trajectory_points_中

  for (size_t i = 0; i < headings.size(); i++) {
    TrajectoryPoint trajectory_pt;
    trajectory_pt.x = xy_points[i].first;
    trajectory_pt.y = xy_points[i].second;
    trajectory_pt.v = 10.0;
    trajectory_pt.a = 0.0;
    trajectory_pt.heading = headings[i];
    trajectory_pt.kappa = kappas[i];

    planning_published_trajectory.trajectory_points.push_back(trajectory_pt);
  }

  trajectory_points_ = planning_published_trajectory.trajectory_points;

9.发布消息：

①定义一个目标点target_point_，目标点为参考轨迹上，距离车辆当前位置最近的点；

②计算速度误差：目标点对应的速度 - 车辆当前的速度

③计算横摆角误差：车辆当前横摆角 - 目标点对应的横摆角

④发布控制指令

  double acceleration_cmd = 0.0;
  double yaw_cmd = 0.0;
  ros::Rate loop_rate(100);  // 设置程序循环的周期是100/s

  while(ros::ok()) {
    
    TrajectoryPoint target_point_;

    target_point_ = QueryNearestPointByPosition(vehicle_state_.x, vehicle_state_.y);

    // 速度误差
    double v_err = target_point_.v - vehicle_state_.v;                

    // 摇摆角误差
    double yaw_err = vehicle_state_.heading - target_point_.heading; 
    if(yaw_err > M_PI / 6)  yaw_err = M_PI / 6;
    else  if(yaw_err < -M_PI / 6) yaw_err = -M_PI / 6;

    // 每循环20次，打印一次信息
    if (cnt % 20 == 0) {
      cout << "start_heading: " << vehicle_state_.start_heading << endl;
      cout << "heading: " << vehicle_state_.heading << endl;
      cout << "v_err: " << v_err << endl;
      cout << "yaw_err: " << yaw_err << endl;
      cout << "control_cmd.target_wheel_angle: " << control_cmd.target_wheel_angle << endl;
    }

    // 加速度指令和横摆角指令
    acceleration_cmd = speed_pid_controller.Control(v_err, 0.01);
    yaw_cmd = yaw_pid_controller.Control(yaw_err, 0.01);    
    
    // 将加速度指令和横摆角指令更新到control_cmd中
    control_cmd.acceleration_pct = acceleration_cmd;
    control_cmd.target_wheel_angle = yaw_cmd;
    
    // 发布控制指令
    control_pub.publish(control_cmd);
    ros::spinOnce();
    loop_rate.sleep();  // // 设置程序循环的周期是100/s
    cnt++;
  }

  return 0;
}