本文将对Apollo的纵向控制器进行讲解，看完本文，你将会对百度Apollo的纵向控制有更深的理解
前面文章：
Apollo9.0 PNC源码学习之Control模块（一）
Apollo9.0 PNC源码学习之Control模块（二）

1 纵向控制器简介

control-controller-lon-based-pid-controller 插件包是基于 PID 控制器进行车辆纵向控制计算的控制器实现，车辆纵向控制是在 Frenet 坐标系下，沿着道路参考线切线的方向，控制车辆的位置、速度、加速度按照规划轨迹线的参考位置、参考速度行驶

control-controller-lon-based-pid-controller 插件主要包含LonController的实现文件， LonController 是继承 ControlTask 的子类，实现 Init、ComputeControlCommand、Reset、Name 等方法。主要的计算逻辑在 ComputeControlCommand方法，输入是规划轨迹线（apollo::planning::ADCTrajectory），车体位置信息（apollo::localization::LocalizationEstimate），车体姿态信息（VehicleStateProvider），车体底盘信息（apollo::canbus::Chassis）等，通过求解纵向误差（位置误差，速度误差）、期望的车辆加速度，加上两级的PID控制器的控制量，可以选择是否需要增加坡度加速度补偿，如果车辆在接近规划轨迹线终点时，可以设置接近终点的加速度进行准确停车，如果车辆前方长时间停着行人，也可以配置对应的加速度作用车辆，求解完成加速度后，然后查询车辆标定表转化成车辆的控制接口如油门踏板或刹车踏板，输入给车辆，完成 1 次控制。

1.1 纵向误差计算

纵向误差计算在 ComputeLongitudinalErrors() 方法中，核心思想是通过绝对时间的查询方法找到规划轨迹线上的点作为控制参考的目标点，然后通过最优距离计算方法（apollo::control::TrajectoryAnalyzer::QueryMatchedPathPoint）找到当前车辆位置在轨迹线上的最佳匹配点（规划线上的点matched_point），将车辆当前位置的坐标（VRF坐标，车体坐标系），投影到最佳匹配轨迹点坐标系（Frenet），求出当前车辆位置投影到S轴（Frenet纵向方向）的距离（PathPoint.s)（这个距离是相对于规划起点定义的），这样纵向误差的计算就在相同的规划轨迹线的坐标系下进行计算，即实际的纵向位置（Frenet:S轴）误差=规划轨迹线的参考点纵向距离（TrajectoryPoint.s）-当前车辆投影在轨迹线上的匹配点纵向距离（s_matched.s），轨迹线参考坐标的处理参考 apollo::control::TrajectoryAnalyzer::ToTrajectoryFrame 方法（modules/control/control_component/control_task_base/common/trajectory_analyzer.cc）

1.2 PID控制器

PID方法在之前文章中详细介绍过，PIDController的方法有：

• Init（初始化）
• SetPID（设置p,i,d参数）
• Reset（重置）
• Reset_integral（积分量重置）
• Control（PID计算）

调试时可以先调kp，从小到大逐渐增加，在适当增加ki消除，有必要时增加kd，提升作用系统的调节速度。先调速度环 speed_pid ，再适当增加位置环 station_pid

1.3 车辆标定表

Apollo 支持的线控车辆主要的控制接口是基于油门、刹车接口，油门/刹车控制量与车辆的行驶速度是线性相关的，这个相关性车辆可能无法直接提供相应的数据或计算公式，为了通过线控接口比较准确的控制车辆速度，需要进行作用量与观测值的定量标定，获得相关性的关系。选取车辆在不同车速下，进行加速或减速直线行驶，记录当前时刻的车体纵向的加速度。如此往复，记录不同速度下，急加速，缓加速，急减速，缓减速等，形成一个标定表，如 modules/control/control_component/conf/calibration_table.pb.txt 所示，打印出如下图所示。
刹车标定表
油门标定表

1.4 模块输入输出与配置

control/controllers/lon_based_pid_controller/
├── conf/                                                 // 控制器配置参数文件
├── docs/                                                 // 文档相关
├── longitudinal_controller_test/                         // 单元测试数据
├── proto
│   ├── BUILD
│   └── lon_based_pid_controller_conf.proto               // 控制器配置参数定义
├── BUILD                                                 // 规则构建文件
├── cyberfile.xml                                         // 插件包管理配置文件
├── lon_controller.cc                                     // 控制器实现文件
├── lon_controller.h                                      // 控制器实现文件
├── lon_controller_test.cc                                // 控制器单元测试文件
├── plugins.xml                                           // 插件配置文件
└── README_cn.md                                          // 说明文档

输入：

Channel名称	类型	描述
/apollo/planning	apollo::planning::ADCTrajectory	车辆规划轨迹线信息（轨迹点信息），control_component 订阅此消息， LonController 继承 ControlTask 基类方法 ComputeControlCommand 传入参数
/apollo/localization/pose	apollo::localization::LocalizationEstimate	车辆定位信息（世界坐标系位置），control_component 订阅此消息，LonController 继承 ControlTask 基类方法 ComputeControlCommand 传入参数
/apollo/canbus/chassis	apollo::canbus::Chassis	车辆底盘信息（底盘车速），control_component订阅此消息， LonController 继承 ControlTask 基类方法 ComputeControlCommand 传入参数
-	apollo::common::VehicleState	车身姿态信息（车身俯仰角）
/apollo/control	apollo::control::ControlCommand	车辆控制指令（方向盘控制指令），control_component 订阅此消息， LonController 继承 ControlTask 基类方法 ComputeControlCommand 传入参数

输出：

Channel名称	类型	描述
/apollo/control	apollo::control::ControlCommand	车辆的控制指令：油门、刹车指令

配置文件：

文件路径	类型/结构	说明
modules/control/control_component/conf/pipeline.pb.txt	apollo::control::ControlPipeline	ControlComponent 的配置文件
modules/control/control_component/conf/control.conf	command line flags	命令行参数配置，配置全局的 flag 变量
modules/control/controllers/lon_based_pid_controller/conf/controller_conf.pb.txt	apollo::control::LonBasedPidControllerConf	PID 纵向控制器配置文件

Flags：

flagfile	类型	描述
modules/control/control_component/common/control_gflags.cc	flags	定义全局的 flag 变量在 LonController 使用，通过 control.conf 进行配置
modules/control/control_component/common/control_gflags.h	declare	flags 声明文件

2 纵向控制器源码解析

lon_controller.h

#pragma once

#include <memory>
#include <string>
#include <vector>

#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/control/controllers/lon_based_pid_controller/proto/lon_based_pid_controller_conf.pb.h"

#include "cyber/cyber.h"
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/common/filters/digital_filter.h"
#include "modules/common/filters/digital_filter_coefficients.h"
#include "modules/control/control_component/controller_task_base/common/interpolation_2d.h"
#include "modules/control/control_component/controller_task_base/common/leadlag_controller.h"
#include "modules/control/control_component/controller_task_base/common/pid_controller.h"
#include "modules/control/control_component/controller_task_base/common/trajectory_analyzer.h"
#include "modules/control/control_component/controller_task_base/control_task.h"
#include "modules/control/controllers/lon_based_pid_controller/util/check_pit.h"

/**
 * @namespace apollo::control
 * @brief apollo::control
 */
namespace apollo {
namespace control {

// LonController类，纵向控制器, 来计算 brake/throttle 值
class LonController : public ControlTask {
 public:

  LonController();

  virtual ~LonController();

  // Init()函数初始化纵向控制器
  // 参数：injector车辆当前状态信息，将其读取到LonController类成员变量injector_里
  common::Status Init(std::shared_ptr<DependencyInjector> injector) override;

  /**
   * @brief compute brake / throttle values based on current vehicle status
   *        and target trajectory
   * @param localization vehicle location
   * @param chassis vehicle status e.g., speed, acceleration
   * @param trajectory trajectory generated by planning
   * @param cmd control command
   * @return Status computation status
   */
  // 计算 刹车/油门值基于当前车辆的状态和目标轨迹
  // 参数：车辆定位信息 指针localization
  // 参数：chassis车辆底盘反馈状态信息 指针chassis
  // 参数：规划发布的轨迹信息 指针trajectory
  // 参数：控制指令 指针cmd，实际上是计算出的控制指令往cmd里填充
  // 返回计算状态码
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd) override;

  // 重置控制器
  common::Status Reset() override;

  // 停止纵向控制器
  void Stop() override;

  // override说明在基类中接口已经被声明为虚函数，调用的是派生类里的定义
  std::string Name() const override;

 protected:
  // 计算纵向误差函数
  // 参数：轨迹信息指针trajectory
  // 参数：预览时间preview_time
  // 参数：控制周期ts
  // 参数：调试信息指针debug用来存放计算的纵向误差信息
  // 该函数在control_component.cc中被调用
  void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory,
                                 const double preview_time, const double ts,
                                 SimpleLongitudinalDebug *debug);

  // 根据规划发布的轨迹msg寻找轨迹点上接下来的第一个停车点
  // 停车点信息存到debug里，这个参数又是用来装结果的
  void GetPathRemain(SimpleLongitudinalDebug *debug);

  std::shared_ptr<DependencyInjector> injector_;

 private:
  // 设置Pitch车辆俯仰角
  // 参数：lon_controller_conf是纵向控制器配置类对象，该类由modules/control/proto/.proto文件生成
  // 从该对象中读取截至频率，控制周期等参数来设置pitch角滤波器
  // pitch角用来进行车辆的坡道补偿，默认坡道补偿是关闭的
  void SetDigitalFilterPitchAngle();
  // 加载控制标定表函数
  // 参数：lon_controller_conf是纵向控制器配置类对象，该类由modules/control/proto/.proto文件生成
  // 从纵向控制器配置对象中读取车速-加速度-控制百分数标定表
  void InitControlCalibrationTable();
  // 设置数字滤波器函数
  // 参数：控制周期ts
  // 参数：截至频率cutoff_freq
  // 参数：数字滤波器类对象digital_filter，前面两项参数就是为了设置这个对象
  void SetDigitalFilter(double ts, double cutoff_freq,
                        common::DigitalFilter *digital_filter);
  // 到达目标是否停止
  bool IsStopByDestination(SimpleLongitudinalDebug *debug);
  // 遇到行人是否停止
  bool IsPedestrianStopLongTerm(SimpleLongitudinalDebug *debug);
  // 完全停止
  bool IsFullStopLongTerm(SimpleLongitudinalDebug *debug);
  // 设置刹车
  void SetParkingBrake(const LonBasedPidControllerConf *conf,
                       control::ControlCommand *control_command);
  // 关闭日志文件函数
  void CloseLogFile();
  // 存放的是定位来的信息，初始化为空指针
  const localization::LocalizationEstimate *localization_ = nullptr;
  // 存放的是来自canbus的车辆状态反馈信息，初始化为空指针
  const canbus::Chassis *chassis_ = nullptr;
  // 实际就是存放标定表及插值功能
  std::unique_ptr<Interpolation2D> control_interpolation_;
  // 存放规划模块发来的轨迹消息
  const planning::ADCTrajectory *trajectory_message_ = nullptr;
  // 用来实现对各种轨迹信息的解析
  std::unique_ptr<TrajectoryAnalyzer> trajectory_analyzer_;
  // 控制器的名称
  std::string name_;
  // 表明控制器是否被初始化成功
  bool controller_initialized_ = false;
  // 定义成员上一时刻的加速度
  double previous_acceleration_ = 0.0;
  // 定义成员上一时刻的参考加速度
  double previous_acceleration_reference_ = 0.0;
  // 纵向控制器里的速度控制器
  PIDController speed_pid_controller_;
  // 纵向控制器里的位置控制器
  PIDController station_pid_controller_;
  //纵向上leadlag控制器默认关闭
  // 用于速度的控制
  LeadlagController speed_leadlag_controller_;
  // 用于位置的控制
  LeadlagController station_leadlag_controller_;
  // 用于存放纵向上的日志信息，默认也关闭csv日志
  FILE *speed_log_file_ = nullptr;
  // 用于对俯仰角pitch进行滤波
  common::DigitalFilter digital_filter_pitch_angle_;
  // 用于存放配置文件中加载进来的控制配置
  LonBasedPidControllerConf lon_based_pidcontroller_conf_;
  // 油门制动标定表
  calibration_table calibration_table_;

  // vehicle parameter
  // 用于存放车辆的实际尺寸参数
  common::VehicleParam vehicle_param_;
  
  bool is_stop_by_pedestrian_ = false;          // 是否被行人停止标志位
  bool is_stop_by_pedestrian_previous_ = false; // 上一次是否被行人停止标志位
  double start_time_ = 0.0;                     // 开始时间
  double wait_time_diff_ = 0.0;                 // 等待时间差

  bool is_full_stop_previous_ = false;          // 上一次是否完全停止标志位
  double is_full_stop_start_time_ = 0.0;        // 完全停止开始时间
  double is_full_stop_wait_time_diff_ = 0.0;    // 完全停止等待时间差

  bool epb_on_change_switch_ = true;            // EPB（紧急停车制动）开关变化标志位
  bool epb_off_change_switch_ = true;           
  int epb_change_count_ = 0;                    // EPB变化计数
  int smode_num_ = 0;                           // S模式数目

  double standstill_narmal_acceleration_ = 0.0; // 静止时的正常加速度
  double stop_gain_acceleration_ = 0.0;         // 停止时的加速度增益
  double max_path_remain_when_stopped_ = 0.0;   // 停止时的最大路径剩余
  bool parking_release_ = false;                // 停车释放标志位
};

// 1.2 当前类声明为插件
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::control::LonController,
                                     ControlTask)

}  // namespace control
}  // namespace apollo

lon_controller.cc

#include "modules/control/controllers/lon_based_pid_controller/lon_controller.h"

#include <algorithm>
#include <utility>

#include "absl/strings/str_cat.h"

#include "cyber/common/log.h"
#include "cyber/time/clock.h"
#include "cyber/time/time.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/control/control_component/common/control_gflags.h"
// #include "modules/localization/common/localization_gflags.h"

namespace apollo {
namespace control {

using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleStateProvider;
using apollo::cyber::Time;
using apollo::external_command::CommandStatusType;
using apollo::planning::ADCTrajectory;
using apollo::planning::StopReasonCode;
// 定义常量重力加速度 9.8
constexpr double GRA_ACC = 9.8;

LonController::LonController() : name_("PID-basesd Longitudinal Controller") {
  // node_.reset(new apollo::cyber::Node("lon_controller"));
  // 是否打开csv debug的功能，默认false
  if (FLAGS_enable_csv_debug) {
    time_t rawtime;
    char name_buffer[80];
    std::time(&rawtime);
    std::tm time_tm;
    localtime_r(&rawtime, &time_tm);
    strftime(name_buffer, 80, "/tmp/speed_log__%F_%H%M%S.csv", &time_tm);
    speed_log_file_ = fopen(name_buffer, "w");
    if (speed_log_file_ == nullptr) {
      AERROR << "Fail to open file:" << name_buffer;
      FLAGS_enable_csv_debug = false;
    }
    if (speed_log_file_ != nullptr) {
      fprintf(speed_log_file_,
              "station_reference,"             // 位置参考
              "station_error,"                 // 位置误差
              "station_error_limited,"         // 位置误差限制
              "preview_station_error,"         // 预瞄位置误差
              "speed_reference,"               // 速度参考
              "speed_error,"                   // 速度误差
              "speed_error_limited,"           // 速度误差限制
              "preview_speed_reference,"       // 预瞄速度参考
              "preview_speed_error,"           // 预瞄速度误差
              "preview_acceleration_reference,"// 预瞄加速度参考
              "acceleration_cmd_closeloop,"    // 闭环加速度cmd
              "acceleration_cmd,"              // 加速度cmd
              "acceleration_lookup,"           // 加速度表现
              "acceleration_lookup_limit,"
              "speed_lookup,"                  // 速度表现
              "calibration_value,"             // 标定表值
              "throttle_cmd,"                  // 油门
              "brake_cmd,"                     // 刹车
              "is_full_stop,"                  // 是否完全停止
              "\r\n");

      fflush(speed_log_file_);
    }
    AINFO << name_ << " used.";
  }
}
// 关闭log日志文件函数主要fclose实现，本身enable_csv_debug就没打开
void LonController::CloseLogFile() {
  if (FLAGS_enable_csv_debug) {
    if (speed_log_file_ != nullptr) {
      fclose(speed_log_file_);
      speed_log_file_ = nullptr;
    }
  }
}
// Stop()函数调用的就是上面的关闭日志文件函数
void LonController::Stop() { CloseLogFile(); }
// 上面的关闭日志文件函数
LonController::~LonController() { CloseLogFile(); }
// 纵向控制器的初始化函数
// 输入参数是DependencyInjector类对象指针injector主要是用来获取车辆状态信息的
Status LonController::Init(std::shared_ptr<DependencyInjector> injector) {
  // 加载纵向控制器参数
  if (!ControlTask::LoadConfig<LonBasedPidControllerConf>(
          &lon_based_pidcontroller_conf_)) {
    AERROR << "failed to load control conf";
    return Status(ErrorCode::CONTROL_INIT_ERROR,
                  "failed to load lon control_conf");
  }
  // 加载油门制动标定表
  if (!ControlTask::LoadCalibrationTable(&calibration_table_)) {
    AERROR << "failed to load calibration table";
    return Status(ErrorCode::CONTROL_INIT_ERROR,
                  "lon failed to load calibration table");
  }
  // 将传进来的参数车辆状态信息injector赋给LonController类数据成员injector_
  injector_ = injector;
  standstill_narmal_acceleration_ =
      -fabs(lon_based_pidcontroller_conf_.standstill_narmal_acceleration());
  stop_gain_acceleration_ =
      -fabs(lon_based_pidcontroller_conf_.stop_gain_acceleration());
  // 控制周期ts
  double ts = lon_based_pidcontroller_conf_.ts();
  // 是否打开超前滞后控制器 默认false
  bool enable_leadlag =
      lon_based_pidcontroller_conf_.enable_reverse_leadlag_compensation();
  // 位置PID控制器的初始化从modules/control/conf/control_conf.pb.txt->lon_controller_conf->station_pid_conf读取
  station_pid_controller_.Init(
      lon_based_pidcontroller_conf_.station_pid_conf());
  // 速度控制器的初始化也是加载control_conf.pb.txt->lon_controller_conf里的相关参数
  speed_pid_controller_.Init(
      lon_based_pidcontroller_conf_.low_speed_pid_conf());
  //如果打开超前滞后控制器 默认关闭
  if (enable_leadlag) {
    station_leadlag_controller_.Init(
        lon_based_pidcontroller_conf_.reverse_station_leadlag_conf(), ts);
    speed_leadlag_controller_.Init(
        lon_based_pidcontroller_conf_.reverse_speed_leadlag_conf(), ts);
  }
  // 车辆的参数从VehicleConfigHelper类的成员函数GetConfig()加载
  vehicle_param_.CopyFrom(
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
  // 从modules/control/conf/control_conf.pb.txt->lon_controller_conf里加载数字滤波器的参数
  // 这个滤波器是用于对pitch角(车辆的俯仰角)进行滤波，pitch角是用于对车辆控制时的坡道补偿
  SetDigitalFilterPitchAngle();
  // 从modules/control/conf/control_conf.pb.txt->lon_controller_conf里加载标定表(不同车速下，加速度到油门/制动标定表)
  // 加载的标定表放入control_interpolation_中
  InitControlCalibrationTable();
  // 纵向控制器被初始化的标志位置为true
  controller_initialized_ = true;

  return Status::OK();
}
// 设置数字滤波器俯仰角
void LonController::SetDigitalFilterPitchAngle() {
  // 配置中读到的参数滤波器的截止频率存到变量cutoff_freq中
  double cutoff_freq =
      lon_based_pidcontroller_conf_.pitch_angle_filter_conf().cutoff_freq();
  // 配置中读到的参数滤波器的采样周期存到变量ts中
  double ts = lon_based_pidcontroller_conf_.ts();
  // 将ts，cutoff_freq作为输入参数调用函数SetDigitalFilter()
  // 将参数全部设置到LonController类的数据成员滤波器类对象digital_filter_pitch_angle_中
  SetDigitalFilter(ts, cutoff_freq, &digital_filter_pitch_angle_);
}
// 初始化标定表
void LonController::InitControlCalibrationTable() {
  AINFO << "Control calibration table size is "
        << calibration_table_.calibration_size();
  // 定义DataType类型变量xyz
	// Apollo类型定义typedef std::vector<std::tuple<double, double, double>> DataType;
	// DataType实际上就是一个三维容器，存放很多组对应的速度，加速度，油门/制动百分数数据
  Interpolation2D::DataType xyz;
  for (const auto &calibration : calibration_table_.calibration()) {
    xyz.push_back(std::make_tuple(calibration.speed(),
                                  calibration.acceleration(),
                                  calibration.command()));
  }
  // std::unique_ptr指针的清空复位，将LonController类数据成员control_interpolation_清空
  control_interpolation_.reset(new Interpolation2D);
  // 将标定表读取到LonController类的数据成员xyz_里(特定车速下的加速度对应的控制量百分数)
  ACHECK(control_interpolation_->Init(xyz))
      << "Fail to load control calibration table";
}
// 计算纵向控制指令
// localization/chassis/planning_published_trajectory都是const只允许读取
Status LonController::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis,
    const planning::ADCTrajectory *planning_published_trajectory,
    control::ControlCommand *cmd) {
  // 将输入的定位，底盘信息分别读取到LonController类的数据成员localization_,chassis_    
  localization_ = localization;
  chassis_ = chassis;
  // 将输入的规划轨迹信息分别读取到LonController类的数据成员trajectory_message_ 
  trajectory_message_ = planning_published_trajectory;
  // 如果标定表为空返回错误信息 
  if (!control_interpolation_) {
    AERROR << "Fail to initialize calibration table.";
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
                  "Fail to initialize calibration table.");
  }
  // 如果规划轨迹信息指针为空或者轨迹分析器里的序号和轨迹message的序号不相等时复位
  if (trajectory_analyzer_ == nullptr ||
      trajectory_analyzer_->seq_num() !=
          trajectory_message_->header().sequence_num()) {
    trajectory_analyzer_.reset(new TrajectoryAnalyzer(trajectory_message_));
  }
  // 定义临时变量debug是cmd.debug.simple_lon_debug
  // 因为cmd是指针所以这样访问数据成员，cmd所属类型包含一个数据成员debug专门用来存放调试信息
  auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
  // 首先将debug清空
  debug->Clear();
  // 初始化刹车，油门控制命令为0
  double brake_cmd = 0.0;
  double throttle_cmd = 0.0;
  // 初始化采样周期ts从lon_controller_conf里读到，lon_controller_conf也是从control_conf.pb.txt中读取到的
  double ts = lon_based_pidcontroller_conf_.ts();
  // preview预览时间=lon_controller_conf读到的纵向预览窗口大小*采样时间ts
  double preview_time = lon_based_pidcontroller_conf_.preview_window() * ts;
  // 默认false
  bool enable_leadlag =
      lon_based_pidcontroller_conf_.enable_reverse_leadlag_compensation();
  // 如果预览窗口读到的是负数则提示错误信息并返回
  if (preview_time < 0.0) {
    const auto error_msg =
        absl::StrCat("Preview time set as: ", preview_time, " less than 0");
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
  }
  // 调用计算纵向误差函数
  // trajectory_analyzer_.get()获得轨迹信息指针用于提供轨迹点的速度加速度，匹配点参考点等信息
  // debug计算得到的误差放入debug中
  ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts,
                            debug);
  // 定义临时变量station_error_limit纵向位置误差限制，默认是2.0m
  double station_error_limit =
      lon_based_pidcontroller_conf_.station_error_limit();
  // 定义临时变量station_error_limited为限幅后的纵向位置误差
  double station_error_limited = 0.0;
  // 如果打开速度-位置预览，默认为true打开speed_station_preview
  // 预览点：当前时间加上预览时间在轨迹上对应的点，车辆将要到达的纵向位置
  // 参考点：当前时间在轨迹上对应的点，车辆此刻应该到达的纵向位置
  // 匹配点：当前时间在轨迹上对应的点，车辆此刻应该到达的纵向位置
  /**
   * 两种位置误差
   * 第一种, preview_station_error=预览点纵向位置 - 匹配点纵向位置
   * 第二种, station_error=参考点纵向位置-匹配点纵向位置
  */
  if (lon_based_pidcontroller_conf_.enable_speed_station_preview()) {
    // 如果打开此开关则采用第一种纵向位置误差，纵向误差的计算结果都是存在debug里
    station_error_limited =
        common::math::Clamp(debug->preview_station_error(),
                            -station_error_limit, station_error_limit);
  } else {
    // 否则采用第二种纵向位置误差，纵向误差的计算结果都是存在debug里
    station_error_limited = common::math::Clamp(
        debug->station_error(), -station_error_limit, station_error_limit);
  }
  // 如果轨迹msg信息指针里的档位信息是R档
  if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) {
    // 检查车是否在坑道里
    if (CheckPit::CheckInPit(debug, &lon_based_pidcontroller_conf_,
                             injector_->vehicle_state()->linear_velocity(),
                             trajectory_message_->is_replan())) {
      ADEBUG << "in pit";
      station_pid_controller_.SetPID(
          lon_based_pidcontroller_conf_.pit_station_pid_conf());
      speed_pid_controller_.SetPID(
          lon_based_pidcontroller_conf_.pit_speed_pid_conf());
    } else { // 不在坑道里
      // 从配置文件加载lon_controller_conf里的倒车PID参数配置加载到位置PID控制器对象station_pid_controller_
      station_pid_controller_.SetPID(
          lon_based_pidcontroller_conf_.reverse_station_pid_conf());
      // 从配置文件加载lon_controller_conf里的倒车PID参数配置加载到速度PID控制器对象station_pid_controller_
      speed_pid_controller_.SetPID(
          lon_based_pidcontroller_conf_.reverse_speed_pid_conf());
    }
    // 如果打开enable_leadlag超前滞后控制器，默认是关闭
    if (enable_leadlag) {
      station_leadlag_controller_.SetLeadlag(
          lon_based_pidcontroller_conf_.reverse_station_leadlag_conf());
      speed_leadlag_controller_.SetLeadlag(
          lon_based_pidcontroller_conf_.reverse_speed_leadlag_conf());
    }
  } else if (injector_->vehicle_state()->linear_velocity() <=
             lon_based_pidcontroller_conf_.switch_speed()) { //若非R档 且 当前车速<=高低速切换的转换速度switch_speed
    // 检查是否在坑道里
    if (CheckPit::CheckInPit(debug, &lon_based_pidcontroller_conf_,
                             injector_->vehicle_state()->linear_velocity(),
                             trajectory_message_->is_replan())) {
      ADEBUG << "in pit";
      station_pid_controller_.SetPID(
          lon_based_pidcontroller_conf_.pit_station_pid_conf());
      speed_pid_controller_.SetPID(
          lon_based_pidcontroller_conf_.pit_speed_pid_conf());
    } else {
      station_pid_controller_.SetPID(
          lon_based_pidcontroller_conf_.station_pid_conf());
      // 速度PID控制器对象speed_pid_controller_加载控制配置文件中低速PID参数
      speed_pid_controller_.SetPID(
          lon_based_pidcontroller_conf_.low_speed_pid_conf());
    }
  } else {
    station_pid_controller_.SetPID(
        lon_based_pidcontroller_conf_.station_pid_conf());
    // 速度PID控制器对象speed_pid_controller_加载控制配置文件中高速PID参数
    speed_pid_controller_.SetPID(
        lon_based_pidcontroller_conf_.high_speed_pid_conf());
  }
  // 非R档情况下，定义临时变量speed_offset速度偏差
  // 速度偏差=位置控制器根据(限幅后位置误差，采样周期)计算出控制量即速度
  double speed_offset =
      station_pid_controller_.Control(station_error_limited, ts);
  if (enable_leadlag) {// 默认false
    speed_offset = station_leadlag_controller_.Control(speed_offset, ts);
  }
  // 定义一个临时变量速度控制器的输入speed_controller_input为0
  double speed_controller_input = 0.0;
  // 从配置文件加载速度控制器输入限幅
  double speed_controller_input_limit =
      lon_based_pidcontroller_conf_.speed_controller_input_limit();
  // 经过限幅之后的速度控制器的输入
  double speed_controller_input_limited = 0.0;
  // 打开speed_station_preview
  if (lon_based_pidcontroller_conf_.enable_speed_station_preview()) {
    // 速度控制器的输入 = 位置控制器计算出的speed_offset + 当前时间向前加上预览时间在轨迹上的对应点的速度和当前车速的偏差
    speed_controller_input = speed_offset + debug->preview_speed_error();
  } else { // 速度控制器的输入 = 位置控制器计算出的speed_offset + 参考点车速和当前车速的偏差
    speed_controller_input = speed_offset + debug->speed_error();
  }
  // 计算得到的速度控制器的输入再进行限幅
  speed_controller_input_limited =
      common::math::Clamp(speed_controller_input, -speed_controller_input_limit,
                          speed_controller_input_limit);
  // 定义临时变量acceleration_cmd_closeloop闭环加速度指令初始化为0
  double acceleration_cmd_closeloop = 0.0;
  // 闭环的加速度指令就等于速度PID控制器根据速度控制器的输入，以及采样周期去计算
  acceleration_cmd_closeloop =
      speed_pid_controller_.Control(speed_controller_input_limited, ts);
  // 将速度PID控制器中积分器的饱和状态设置到debug.pid_saturation_status
  debug->set_pid_saturation_status(
      speed_pid_controller_.IntegratorSaturationStatus());
  if (enable_leadlag) {
    acceleration_cmd_closeloop =
        speed_leadlag_controller_.Control(acceleration_cmd_closeloop, ts);
    debug->set_leadlag_saturation_status(
        speed_leadlag_controller_.InnerstateSaturationStatus());
  }
  // 汽车空档
  if (chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
    speed_pid_controller_.Reset_integral();  // 重置积分
    station_pid_controller_.Reset_integral();// 重置积分
  }
  // 汽车俯仰角弧度
  double vehicle_pitch_rad =
      digital_filter_pitch_angle_.Filter(injector_->vehicle_state()->pitch());
  // 汽车俯仰角
  double vehicle_pitch =
      vehicle_pitch_rad * 180 / M_PI + FLAGS_pitch_offset_deg;
  ADEBUG << "[LON]vehicle_pitch is " << vehicle_pitch;
  // 将vehicle_pitch放入debug里
  debug->set_vehicle_pitch(vehicle_pitch);
  // TODO(ALL): confirm the slope_offset_compensation whether is positive or not
  // when vehicle move uphill
  // Resume: uphill: + , downhill: -
  // 定义斜坡补偿加速度 = use_opposite_slope_compensation *(重力加速度 * 车辆俯仰角的正弦值)得到斜坡加速度补偿
  double slope_offset_compensation =
      lon_based_pidcontroller_conf_.use_opposite_slope_compensation() *
      GRA_ACC *
      std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180);
  // 判断坡道补偿加速度是否为非数NaN，当浮点数过小下溢就可能出现NaN非数
  if (std::isnan(slope_offset_compensation)) {
    slope_offset_compensation = 0;
  }
  // 将斜坡补偿加速度设置到debug里，debug.slope_offset_compensation=slope_offset_compensation
  debug->set_slope_offset_compensation(slope_offset_compensation);
  // 总的加速度指令 = 闭环加速度指令 + 预览参考加速度 + 坡道补偿加速度(如果打开坡道补偿的话)
  double acceleration_cmd =
      acceleration_cmd_closeloop + debug->preview_acceleration_reference() +
      lon_based_pidcontroller_conf_.enable_slope_offset() *
          debug->slope_offset_compensation();

  // Check the steer command in reverse trajectory if the current steer target
  // is larger than previous target, free the acceleration command, wait for
  // the current steer target
  
  double current_steer_interval =
      cmd->steering_target() - chassis->steering_percentage();
  // 转向检查 + 未知轨迹 + 当前转向间隔 > steer_cmd_interval
  if (lon_based_pidcontroller_conf_.use_steering_check() &&
      (trajectory_message_->trajectory_type() ==
       apollo::planning::ADCTrajectory::UNKNOWN) &&
      std::abs(current_steer_interval) >
          lon_based_pidcontroller_conf_.steer_cmd_interval()) {
    ADEBUG << "steering_target is " << cmd->steering_target()
           << " steering_percentage is " << chassis->steering_percentage();
    ADEBUG << "Steer cmd interval is larger than "
           << lon_based_pidcontroller_conf_.steer_cmd_interval();
    // 重置速度位置PID的积分，加速度指令设置为0
    speed_pid_controller_.Reset_integral();
    station_pid_controller_.Reset_integral();
    acceleration_cmd = 0;
    // 等待转向 true
    debug->set_is_wait_steer(true);
  } else { // 等待转向 false
    debug->set_is_wait_steer(false);
  }
  // current_steer_interval加入调试信息中
  debug->set_current_steer_interval(current_steer_interval);

  // At near-stop stage, replace the brake control command with the standstill
  // acceleration if the former is even softer than the latter
  // 设置debug的is_full_stop标志位为false
  debug->set_is_full_stop(false);
  debug->set_is_full_stop_soft(false);
  auto previous_full_stop =
      injector_->Get_previous_lon_debug_info()->is_full_stop();
  // 获取停车点的一个函数，后面介绍，找到当前规划模块发布的轨迹msg里的第一个v,a都小于一个很小值的点作为停车点
  // 找到的这个停车点的纵向位置和当前车辆纵向位置的偏差设置到debug里面去，debug.path_remain()
  GetPathRemain(debug);
  IsStopByDestination(debug);
  IsPedestrianStopLongTerm(debug);
  // 使用预览参考 + 预览加速度参考 <= 停止的最大加速度 + 预览速度参考 <= 停止的最大速度 + 轨迹不在开放空间
  if (lon_based_pidcontroller_conf_.use_preview_reference_check() &&
      (std::fabs(debug->preview_acceleration_reference()) <=
       FLAGS_max_acceleration_when_stopped) &&
      std::fabs(debug->preview_speed_reference()) <=
          vehicle_param_.max_abs_speed_when_stopped() &&
      trajectory_message_->trajectory_type() != ADCTrajectory::OPEN_SPACE) {
    // 因为目标地和行人停车
    if (debug->is_stop_reason_by_destination() ||
        debug->is_stop_reason_by_prdestrian()) {
      // 完全停车
      debug->set_is_full_stop(true);
      ADEBUG << "Into full stop within preview acc and reference speed, "
             << "is_full_stop is " << debug->is_full_stop();
    } else {
      debug->set_is_full_stop_soft(true);
      ADEBUG << "Into full stop soft within preview acc and reference speed, "
             << "is_full_stop_soft is " << debug->is_full_stop_soft();
    }
  }
  // 启用预览完全停车
  if (!previous_full_stop) { // 停车最大留用距离
    max_path_remain_when_stopped_ = FLAGS_max_path_remain_when_stopped;
  } else {
    max_path_remain_when_stopped_ =
        FLAGS_max_path_remain_when_stopped +
        lon_based_pidcontroller_conf_.full_stop_path_remain_gain();
  }
  // 前进档+还没有到达停车最大留用距离 || 倒档 + 还没有到达停车最大留用距离
  if (((trajectory_message_->gear() == Chassis::GEAR_DRIVE) &&
       debug->path_remain() < max_path_remain_when_stopped_) ||
      ((trajectory_message_->gear() == Chassis::GEAR_REVERSE) &&
       debug->path_remain() > -max_path_remain_when_stopped_)) {
    ADEBUG << "Into full stop decision by path remain.";
    if (debug->is_stop_reason_by_destination() ||
        debug->is_stop_reason_by_prdestrian()) {
      debug->set_is_full_stop(true);
      // 当前路径留用距离
      ADEBUG << "Current path remain distance: " << debug->path_remain()
             << " is within max_path_remain threshold: "
             << max_path_remain_when_stopped_
             << ", into full stop because vehicle is in destination: "
             << debug->is_stop_reason_by_destination()
             << " or pedestrian is in long time stop: "
             << debug->is_stop_reason_by_prdestrian()
             << "is_full_stop flag: " << debug->is_full_stop();
    } else {
      debug->set_is_full_stop_soft(true);
      ADEBUG << "Current path remain distance: " << debug->path_remain()
             << " is within max_path_remain threshold: "
             << max_path_remain_when_stopped_
             << ", but not into full stop because stop not in destination: "
             << debug->is_stop_reason_by_destination()
             << " or pedestrian is not long time stop: "
             << debug->is_stop_reason_by_prdestrian()
             << "is_full_stop_soft flag: " << debug->is_full_stop_soft();
    }
    // 当前线速度 < 停止最大车速 && 因为行人停止
    if (injector_->vehicle_state()->linear_velocity() <
            vehicle_param_.max_abs_speed_when_stopped() &&
        debug->is_stop_reason_by_prdestrian()) {
      ADEBUG << "Current stop is for long time pedestrian stop, "
             << debug->is_stop_reason_by_prdestrian();
      // 完全停车
      debug->set_is_full_stop(true);
    }
  } else {
    ADEBUG << "Not into full stop decision by path remain.";
  }
  // 完全停车
  if (debug->is_full_stop()) { //R档略过
    acceleration_cmd =
        (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
            ? std::max(acceleration_cmd, //standstill_acceleration默认为-0.3 m/s^2
                       -lon_based_pidcontroller_conf_.standstill_acceleration())
            : std::min(acceleration_cmd, //非R档取控制器计算加速度指令，standstill减速度中最小值
                       lon_based_pidcontroller_conf_.standstill_acceleration());
    // 速度、位置PID积分为0
    speed_pid_controller_.Reset_integral();
    station_pid_controller_.Reset_integral();
  }
  // 如果车辆已完全停止
  if (debug->is_full_stop_soft()) {
    // 如果当前档位不是倒车档
    if (chassis->gear_location() != canbus::Chassis::GEAR_REVERSE) {
      // 根据加速度命令的正负性确定下一步加速度命令
      acceleration_cmd =
          (acceleration_cmd >= 0)       ? standstill_narmal_acceleration_
          : (debug->path_remain() >= 0) ? acceleration_cmd
          : (trajectory_message_->trajectory_type() != ADCTrajectory::NORMAL)
              ? (acceleration_cmd + stop_gain_acceleration_)
              : (acceleration_cmd + standstill_narmal_acceleration_);
    } else {
      // 如果当前档位是倒车档
      // 根据加速度命令的正负性确定下一步加速度命令
      acceleration_cmd =
          (acceleration_cmd <= 0)       ? -standstill_narmal_acceleration_
          : (debug->path_remain() <= 0) ? acceleration_cmd
          : (trajectory_message_->trajectory_type() != ADCTrajectory::NORMAL)
              ? (acceleration_cmd - stop_gain_acceleration_)
              : (acceleration_cmd - standstill_narmal_acceleration_);
    }
    // 重置速度PID控制器的积分项
    speed_pid_controller_.Reset_integral();
    // 重置位置PID控制器的积分项
    station_pid_controller_.Reset_integral();
  }
  // 定义油门指令的下边界
  double throttle_lowerbound =
      std::max(vehicle_param_.throttle_deadzone(),
               lon_based_pidcontroller_conf_.throttle_minimum_action());
  // 定义刹车指令的下边界
  double brake_lowerbound =
      std::max(vehicle_param_.brake_deadzone(),
               lon_based_pidcontroller_conf_.brake_minimum_action());
  // 定义临时变量calibration_value标定值初始化为0
  double calibration_value = 0.0;
  // 要用来查表加速度，若R档为加速度控制指令取反，非R档保持加速度控制指令
  double acceleration_lookup =
      (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
          ? -acceleration_cmd
          : acceleration_cmd;
  // 加速度查表限制 = 最大加速度 + 斜坡补偿
  double acceleration_lookup_limited =
      vehicle_param_.max_acceleration() +
      lon_based_pidcontroller_conf_.enable_slope_offset() *
          debug->slope_offset_compensation();
  double acceleration_lookup_limit = 0.0;
  // 使用加速度查表限制
  if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
    acceleration_lookup_limit =
        (acceleration_lookup > acceleration_lookup_limited)
            ? acceleration_lookup_limited
            : acceleration_lookup;
  }
  // 是否用预览点速度来查标定表(车速-加速度-控制指令百分数) false
  if (FLAGS_use_preview_speed_for_table) {
    if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
      calibration_value = control_interpolation_->Interpolate(
          std::make_pair(std::fabs(debug->preview_speed_reference()),
                         acceleration_lookup_limit));
    } else { // 用速度加速度根据标定表线性插值得到控制量百分数calibration_value
      calibration_value = control_interpolation_->Interpolate(std::make_pair(
          std::fabs(debug->preview_speed_reference()), acceleration_lookup));
    }
  } else { 
    if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
      calibration_value = control_interpolation_->Interpolate(std::make_pair(
          std::fabs(injector_->vehicle_state()->linear_velocity()),
          acceleration_lookup_limit));
    } else {
      calibration_value = control_interpolation_->Interpolate(std::make_pair(
          std::fabs(injector_->vehicle_state()->linear_velocity()),
          acceleration_lookup));
    }
  }
  // 如果请求查表加速度>=0
  if (acceleration_lookup >= 0) {
    // 如果计算得到的控制百分数>=0
    if (calibration_value >= 0) {
      // 设置油门控制百分数，为油门下边界和查表得到的控制百分数之间的较大值
      throttle_cmd = std::max(calibration_value, throttle_lowerbound);
    } else {// 如果计算得到的控制百分数<0,但是加速度又大于0
      // 设置油门控制百分数为油门下边界
      throttle_cmd = throttle_lowerbound;
    }
    // 刹车指令为0，如果要加速
    brake_cmd = 0.0;
  } else {
    // 如果请求的查表加速度<0,油门指令置0
    throttle_cmd = 0.0;
    // 如果计算得到的控制百分数>=0
    if (calibration_value >= 0) {
      //刹车指令就取刹车下边界
      brake_cmd = brake_lowerbound;
    } else {
      // 如果计算得到的控制百分数<0 刹车指令就取标定值相反数和刹车下边界里较大值，其实就是取标定值进行限幅不能太小
      brake_cmd = std::max(-calibration_value, brake_lowerbound);
    }
  }
  // 如果启用了车辆电子驻车（EPB）
  if (FLAGS_use_vehicle_epb) {
    // 输出调试信息，表示进入使用车辆EPB的逻辑
    ADEBUG << "Into use vehicle epb.";
    // 如果加速度查找大于等于0
    if (acceleration_lookup >= 0) {
      // 如果坡道偏移补偿大于0
      if (debug->slope_offset_compensation() > 0) {
        // 如果加速度查找大于坡道偏移补偿
        if (acceleration_lookup > debug->slope_offset_compensation()) {
          // 设置EPB释放标志为true
          parking_release_ = true;
        }
      } else {// 如果坡道偏移补偿小于等于0，则直接设置EPB释放标志为tru
        parking_release_ = true;
      }
      // 如果当前处于停车制动状态且EPB释放标志为true
      if (chassis->parking_brake() && parking_release_) {
        // 输出调试信息，表示进入停车制动释放逻辑
        ADEBUG << "Into park brake release.";
        // 设置制动指令中的停车制动为false
        cmd->set_parking_brake(false);
        // 设置车辆停车制动
        SetParkingBrake(&lon_based_pidcontroller_conf_, cmd);
      }
    } else {
      // 如果加速度查找小于0  设置制动指令中的停车制动为false
      cmd->set_parking_brake(false);
      // 如果当前处于完全停止状态且为长期停止
      if (debug->is_full_stop() && IsFullStopLongTerm(debug)) {
        // 输出调试信息，表示进入停车制动触发逻辑
        ADEBUG << "Into park brake trigger.";
        // 设置制动指令中的停车制动为true
        cmd->set_parking_brake(true);
        // 如果当前处于停车制动状态
        if (chassis->parking_brake()) {
          brake_cmd = 0.0; // 将制动命令设置为0.0
        }
      }
    }
  }
  // 将被限制的纵向位置误差设置到debug.station_error_limited
  debug->set_station_error_limited(station_error_limited);
  // 位置控制器的输出
  debug->set_speed_offset(speed_offset);
  // 被限幅的速度控制器的输入
  debug->set_speed_controller_input_limited(speed_controller_input_limited);
  // 计算到的加速度指令
  debug->set_acceleration_cmd(acceleration_cmd);
  // 计算到的油门指令
  debug->set_throttle_cmd(throttle_cmd);
  // 计算到的刹车指令
  debug->set_brake_cmd(brake_cmd);
  // 去查标定表的请求加速度
  debug->set_acceleration_lookup(acceleration_lookup);
  debug->set_acceleration_lookup_limit(acceleration_lookup_limit);
  // 去查标定表的速度将chassis反馈值设置到debug.speed_lookup
  debug->set_speed_lookup(injector_->vehicle_state()->linear_velocity());
  // 标定表中查到的控制百分数值
  debug->set_calibration_value(calibration_value);
  // 闭环反馈速度控制器计算得到的控制量加速度
  debug->set_acceleration_cmd_closeloop(acceleration_cmd_closeloop);
  // 如果打开csv日志记录默认false
  if (FLAGS_enable_csv_debug && speed_log_file_ != nullptr) {
    fprintf(speed_log_file_,
            "%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f,"
            "%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %d,\r\n",
            debug->station_reference(), debug->station_error(),
            station_error_limited, debug->preview_station_error(),
            debug->speed_reference(), debug->speed_error(),
            speed_controller_input_limited, debug->preview_speed_reference(),
            debug->preview_speed_error(),
            debug->preview_acceleration_reference(), acceleration_cmd_closeloop,
            acceleration_cmd, debug->acceleration_lookup(),
            debug->acceleration_lookup_limit(), debug->speed_lookup(),
            calibration_value, throttle_cmd, brake_cmd, debug->is_full_stop());
  }

  // if the car is driven by acceleration, disgard the cmd->throttle and brake
  // 如果车辆是以加速度驱动，那么可以忽略下面的油门，制动指令值
  cmd->set_throttle(throttle_cmd);
  cmd->set_brake(brake_cmd);
  // 使用加速度查找限制
  if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
    cmd->set_acceleration(acceleration_lookup_limit);
  } else {
    cmd->set_acceleration(acceleration_cmd);
  }
  // 纵向速度 <= 最大停车速度 || 空档 || 档位与轨迹消息档位一致
  if (std::fabs(injector_->vehicle_state()->linear_velocity()) <=
          vehicle_param_.max_abs_speed_when_stopped() ||
      chassis->gear_location() == trajectory_message_->gear() ||
      chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
    // 若车辆处于停车或N档时下发规划发布的轨迹msg里的档位
    cmd->set_gear_location(trajectory_message_->gear());
  } else {
    // 若车辆不处于停车且不在N档时下发chassis反馈的车辆实际档位
    cmd->set_gear_location(chassis->gear_location());
  }

  return Status::OK();
}
// 重置速度、位置pid
Status LonController::Reset() {
  speed_pid_controller_.Reset();
  station_pid_controller_.Reset();
  return Status::OK();
}

std::string LonController::Name() const { return name_; }
// 计算纵向误差
void LonController::ComputeLongitudinalErrors(
    const TrajectoryAnalyzer *trajectory_analyzer, const double preview_time,
    const double ts, SimpleLongitudinalDebug *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
  // 输入参数规划发布轨迹信息TrajectoryAnalyzer类指针对象trajectory_analyzer
	// 输入参数preview_time,预览时间，在控制配置文件里面设置
	// 输入参数ts，采样时间，在控制配置文件里设置
	// 输入参数debug，SimpleLongitudinalDebug类指针对象debug用来存放计算得到纵向误差信息
  // matched:匹配点，在参考轨迹上距离当前车辆距离最近的点
  double s_matched = 0.0;      // 纵向累积走过的距离沿着参考轨迹
  double s_dot_matched = 0.0;  // 纵向沿着参考轨迹的速度
  double d_matched = 0.0;      // 相对参考轨迹的横向距离
  double d_dot_matched = 0.0;  // 横向距离的变化率
  // 获取车辆状态
  auto vehicle_state = injector_->vehicle_state();
  // 匹配点
  auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(
      vehicle_state->x(), vehicle_state->y());
  // 轨迹信息将当前点x,y,theta,v以及匹配点信息输入，输出当前点的s,d,s',d'
  // 将大地坐标系转化为Frenet坐标
  trajectory_analyzer->ToTrajectoryFrame(
      vehicle_state->x(), vehicle_state->y(), vehicle_state->heading(),
      vehicle_state->linear_velocity(), matched_point, &s_matched,
      &s_dot_matched, &d_matched, &d_dot_matched);

  // double current_control_time = Time::Now().ToSecond();
  // 当前控制时间=当前时间
  double current_control_time = ::apollo::cyber::Clock::NowInSeconds();
  // 预览控制时间=当前时间+预览时间
  double preview_control_time = current_control_time + preview_time;
  // 参考点就是用当前时间去轨迹上查时间最近点
  TrajectoryPoint reference_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(
          current_control_time);
  // 预览点就是去轨迹上查预览时间对应的点，就是当前时间向前看一段时间对应轨迹上的点
  TrajectoryPoint preview_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(
          preview_control_time);
  // 所有的计算结果都是存到debug这个指针对象里
  //debug.matched_point.pathpoint.x=匹配点x
  debug->mutable_current_matched_point()->mutable_path_point()->set_x(
      matched_point.x());
  //debug.matched_point.pathpoint.y=匹配点y
  debug->mutable_current_matched_point()->mutable_path_point()->set_y(
      matched_point.y());
  // debug.reference_point.pathpoint.x=参考点x
  debug->mutable_current_reference_point()->mutable_path_point()->set_x(
      reference_point.path_point().x());
  // debug.reference_point.pathpoint.y=参考点y
  debug->mutable_current_reference_point()->mutable_path_point()->set_y(
      reference_point.path_point().y());
  // debug.preview_point.pathpoint.x=预览点x
  debug->mutable_preview_reference_point()->mutable_path_point()->set_x(
      preview_point.path_point().x());
  // debug.preview_point.pathpoint.y=预览点y
  debug->mutable_preview_reference_point()->mutable_path_point()->set_y(
      preview_point.path_point().y());
  // 打印匹配点，参考点，预览点信息
  ADEBUG << "matched point:" << matched_point.DebugString();
  ADEBUG << "reference point:" << reference_point.DebugString();
  ADEBUG << "preview point:" << preview_point.DebugString();
  // 航向角误差 = 车辆当前状态航向角 - 匹配点的航向角
  // NormalizeAngle角度的规范化，就是将所有角度规范到-pi,pi
  double heading_error = common::math::NormalizeAngle(vehicle_state->heading() -
                                                      matched_point.theta());
  // 纵向速度 = 车辆速度 * cos(当前航向角 - 轨迹上距离最近点航向角)
  double lon_speed = vehicle_state->linear_velocity() * std::cos(heading_error);
  // 纵向加速度 = 车辆加速度 * cos(当前航向角 - 轨迹上距离最近点航向角)
  double lon_acceleration =
      vehicle_state->linear_acceleration() * std::cos(heading_error);
  double one_minus_kappa_lat_error = 1 - reference_point.path_point().kappa() *
                                             vehicle_state->linear_velocity() *
                                             std::sin(heading_error);
  // 计算的相关误差等结果全部存放到指针debug里供类内其他函数调用
  // 参考的纵向位置debug.station_reference=参考点的累积弧长
  debug->set_station_reference(reference_point.path_point().s());
  // 当前的纵向位置debug.current_station=当前点的累积弧长
  debug->set_current_station(s_matched);
  // 纵向位置误差debug.station_error=参考点路径点的累积弧长-匹配点的累积弧长(匹配点就是路径最近点)
  debug->set_station_error(reference_point.path_point().s() - s_matched);
  // 速度参考值就是参考点的速度debug.speed_reference
  debug->set_speed_reference(reference_point.v());
  // 当前速度
  debug->set_current_speed(lon_speed);
  // 速度误差就是参考点速度减匹配点速度 debug.speed_error
  debug->set_speed_error(reference_point.v() - s_dot_matched);
  // 参考点加速度
  debug->set_acceleration_reference(reference_point.a());
  // 纵向加速度
  debug->set_current_acceleration(lon_acceleration);
  // 加速度误差=参考点加速度-纵向加速度/(1-kd)  1-kd由全局坐标转换到Frenet坐标引入，kappa就是曲率
  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->current_acceleration() - previous_acceleration_) / ts;
  // 参考jerk
  debug->set_jerk_reference(jerk_reference);
  // 设置当前jerk
  debug->set_current_jerk(lon_jerk);
  // 加加速度误差=加加速度参考-纵向加加速度/(1-kd)存到debug里
  debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
  // 更新previous_acceleration_reference_
  previous_acceleration_reference_ = debug->acceleration_reference();
  // 更新previous_acceleration_
  previous_acceleration_ = debug->current_acceleration();
  // 设置预览点的相关参数
  debug->set_preview_station_error(preview_point.path_point().s() - s_matched);
  debug->set_preview_speed_error(preview_point.v() - s_dot_matched);
  debug->set_preview_speed_reference(preview_point.v());
  debug->set_preview_acceleration_reference(preview_point.a());
}
// 设置数字滤波器
void LonController::SetDigitalFilter(double ts, double cutoff_freq,
                                     common::DigitalFilter *digital_filter) {
  std::vector<double> denominators;
  std::vector<double> numerators;
  common::LpfCoefficients(ts, cutoff_freq, &denominators, &numerators);
  digital_filter->set_coefficients(denominators, numerators);
}

// TODO(all): Refactor and simplify
// 就是去轨迹点上找第一个符合条件的停止点，并将结果存在debug指针
// 然后快到停车点时，就给一个固定的standstill减速度，配置里默认设置-0.3
void LonController::GetPathRemain(SimpleLongitudinalDebug *debug) {
  // 首先定义初始化了停止点在规划发布轨迹点中的下标为0
  int stop_index = 0;
  // 定义了静态常量速度阈值，速度小于此阈值认为是停止条件之一
  static constexpr double kSpeedThreshold = 1e-3;
  // 定义了静态常量加速度常量 前进档时加速度>此阈值且<0认为是停止条件之一
  static constexpr double kForwardAccThreshold = -1e-2;
  // 定义了静态常量加速度常量 R档时加速度<此阈值且>0认为是停止条件之一
  static constexpr double kBackwardAccThreshold = 1e-1;
  // 定义了静态常量驻车速度，也是判断停车点的一个依据
  static constexpr double kParkingSpeed = 0.1;
  //若规划发布的轨迹信息trajectory_message_中档位为前进档
  if (trajectory_message_->gear() == canbus::Chassis::GEAR_DRIVE) {
    // 这里就是stop_index停车点下标从0开始遍历当前规划发布轨迹所有点
    while (stop_index < trajectory_message_->trajectory_point_size()) {
      // 定义当前此次循环遍历的轨迹点为轨迹msg中下标为stop_index的那个点
      auto &current_trajectory_point =
          trajectory_message_->trajectory_point(stop_index);
      // 若当前遍历的轨迹点速度绝对值 < 速度阈值 且 当前遍历的轨迹点加速度 > 前进加速度阈值 且 当前遍历的轨迹点加速度 < 0
      // 若符合这条件则找到了停车点直接break跳出while遍历循环
      if (fabs(current_trajectory_point.v()) < kSpeedThreshold &&
          current_trajectory_point.a() > kForwardAccThreshold &&
          current_trajectory_point.a() < 0.0) {
        break;
      }
      // 若此次循环不符合上述停车点的判定条件，则遍历规划发布的轨迹msg中的下一个点
      ++stop_index;
    }
  } else {
    // 若为非前进档的停车点下标搜索
    while (stop_index < trajectory_message_->trajectory_point_size()) {
      auto &current_trajectory_point =
          trajectory_message_->trajectory_point(stop_index);
      if (current_trajectory_point.v() > -kSpeedThreshold &&
          current_trajectory_point.a() < kBackwardAccThreshold &&
          current_trajectory_point.a() > 0.0) {
        break;
      }
      ++stop_index;
    }
  }
  // 打印停止点
  ADEBUG << "stop_index is, " << stop_index;
  // 如果到了轨迹msg的最后一个点都没有符合上边符合要求的就将最后一个轨迹点为停止点
  if (stop_index == trajectory_message_->trajectory_point_size()) {
    --stop_index;
    // 若最后一个轨迹点速度绝对值 < 驻车速度阈值
    if (fabs(trajectory_message_->trajectory_point(stop_index).v()) <
        kParkingSpeed) {
      // 显示打印信息，最后一个点被选为停车点
      ADEBUG << "the last point is selected as parking point";
    } else {
      // 否则的话也只是提示终点的速度 > 速度死区而已，停车点仍是选这个，仅仅打印信息不同
      ADEBUG << "the last point found in path and speed > speed_deadzone";
    }
  }
  // 将停车点的纵向位置与当前点的纵向位置之差存到debug.path_remain里
  debug->set_path_remain(
      trajectory_message_->trajectory_point(stop_index).path_point().s() -
      debug->current_station());
}
// 判断是否通过目的地停止
bool LonController::IsStopByDestination(SimpleLongitudinalDebug *debug) {
  // 获取停止原因
  auto stop_reason = trajectory_message_->decision().main_decision().stop();
  // 输出当前停止原因的调试信息
  ADEBUG << "Current stop reason is \n" << stop_reason.DebugString();
  // 输出规划命令状态消息的调试信息
  ADEBUG << "Planning command status msg is \n"
         << injector_->get_planning_command_status()->ShortDebugString();
  // 获取停止原因代码
  StopReasonCode stop_reason_code = stop_reason.reason_code();
  // 如果停止原因是信号、参考结束、预开放空间停止之一，或者规划命令状态为完成，或者决策中含有任务完成标志
  if (stop_reason_code == StopReasonCode::STOP_REASON_SIGNAL ||
      stop_reason_code == StopReasonCode::STOP_REASON_REFERENCE_END ||
      stop_reason_code == StopReasonCode::STOP_REASON_PRE_OPEN_SPACE_STOP ||
      injector_->get_planning_command_status()->status() ==
          CommandStatusType::FINISHED ||
      trajectory_message_->decision().main_decision().has_mission_complete()) {
    // 输出调试信息，表示当前停止原因在目的地范围内
    ADEBUG << "[IsStopByDestination]Current stop reason is in destination.";
    // 设置调试标志，表示停止原因是通过目的地停止的
    debug->set_is_stop_reason_by_destination(true);
    return true;
  }
  // 设置调试标志，表示停止原因不是通过目的地停止的
  debug->set_is_stop_reason_by_destination(false);
  return false;
}
// 判断是否存在长时间行人停止
bool LonController::IsPedestrianStopLongTerm(SimpleLongitudinalDebug *debug) {
  // 获取停止原因
  auto stop_reason = trajectory_message_->decision().main_decision().stop();
  // 输出当前停止原因的调试信息
  ADEBUG << "Current stop reason is \n" << stop_reason.DebugString();
  // 获取停止原因代码
  StopReasonCode stop_reason_code = stop_reason.reason_code();
  // 如果停止原因是行人或障碍物
  if (stop_reason_code == StopReasonCode::STOP_REASON_PEDESTRIAN ||
      stop_reason_code == StopReasonCode::STOP_REASON_OBSTACLE) {
    // 输出调试信息，表示停止原因是行人
    ADEBUG << "[IsPedestrianStopLongTerm]Stop reason for pedestrian.";
    is_stop_by_pedestrian_ = true;// 设置标志，表示通过行人停止
  } else {
    is_stop_by_pedestrian_ = false; // 设置标志，表示不是通过行人停止
  }
  // 输出当前行人停止标志和上次行人停止标志的状态信息
  ADEBUG << "Current is_stop_by_pedestrian: " << is_stop_by_pedestrian_
         << ", is_stop_by_pedestrian_previous: "
         << is_stop_by_pedestrian_previous_;
  // 如果当前是通过行人停止
  if (is_stop_by_pedestrian_) {
    // 如果当前停止不是由行人引起的，而上次停止是由行人引起的
    if (!(is_stop_by_pedestrian_ && is_stop_by_pedestrian_previous_)) {
      // 记录当前时间作为停止开始时间
      start_time_ = ::apollo::cyber::Clock::NowInSeconds();
      ADEBUG << "Stop reason for pedestrian, start time(s) is " << start_time_;
    } else {
      // 上次已经是行人停止，跳过开始时间初始化
      ADEBUG << "Last time stop is already pedestrian, skip start_time init.";
    }
    // 记录当前时间作为停止结束时间
    double end_time = ::apollo::cyber::Clock::NowInSeconds();
    ADEBUG << "Stop reason for pedestrian, current time(s) is " << end_time;
    // 计算停止持续时间
    wait_time_diff_ = end_time - start_time_;
  } else { // 如果不是通过行人停止，则开始时间和持续时间均为0
    start_time_ = 0.0;
    wait_time_diff_ = 0.0;
  }
  // 更新上次行人停止标志
  is_stop_by_pedestrian_previous_ = is_stop_by_pedestrian_;
  // 如果行人停止持续时间超过阈值
  if (wait_time_diff_ > lon_based_pidcontroller_conf_.pedestrian_stop_time()) {
    // 输出调试信息，表示行人停止持续时间超过阈值
    ADEBUG << "Current pedestrian stop lasting time(s) is " << wait_time_diff_
           << ", larger than threshold: "
           << lon_based_pidcontroller_conf_.pedestrian_stop_time();
    // 设置调试标志，表示停止原因是由行人引起的
    debug->set_is_stop_reason_by_prdestrian(true);
    return true;
  } else {
    // 输出调试信息，表示行人停止持续时间未达到阈值
    ADEBUG << "Current pedestrian stop lasting time(s) is " << wait_time_diff_
           << ", not reach the threshold: "
           << lon_based_pidcontroller_conf_.pedestrian_stop_time();
    // 设置调试标志，表示停止原因不是由行人引起的
    debug->set_is_stop_reason_by_prdestrian(false);
    return false;
  }
}
// 判断是否存在长时间完全停止
bool LonController::IsFullStopLongTerm(SimpleLongitudinalDebug *debug) {
  // 如果当前为完全停止状态
  if (debug->is_full_stop()) {
    // 如果当前停止不是由完全停止引起的，而上次停止是由完全停止引起的
    if (debug->is_full_stop() && !is_full_stop_previous_) {
      // 记录当前时间作为完全停止开始时间
      is_full_stop_start_time_ = ::apollo::cyber::Clock::NowInSeconds();
      ADEBUG << "Full stop long term start time(s) is "
             << is_full_stop_start_time_;
    } else {
      // 上次已经是完全停止，跳过开始时间初始化
      ADEBUG << "Last time stop is already full stop, skip start_time init.";
    }
    // 记录当前时间作为完全停止结束时间
    double is_full_stop_start_end_time = ::apollo::cyber::Clock::NowInSeconds();
    // 计算完全停止持续时间
    is_full_stop_wait_time_diff_ =
        is_full_stop_start_end_time - is_full_stop_start_time_;
  } else {// 如果不是完全停止，则开始时间和持续时间均为0
    is_full_stop_start_time_ = 0.0;
    is_full_stop_wait_time_diff_ = 0.0;
  }
  // 更新上次完全停止标志
  is_full_stop_previous_ = debug->is_full_stop();
  // 如果完全停止持续时间超过阈值
  if (is_full_stop_wait_time_diff_ >
      lon_based_pidcontroller_conf_.full_stop_long_time()) {
    // 输出调试信息，表示完全停止持续时间超过阈值
    ADEBUG << "Current full stop lasting time(s) is "
           << is_full_stop_wait_time_diff_ << ", larger than threshold: "
           << lon_based_pidcontroller_conf_.full_stop_long_time();
    return true;
  } else {
    // 输出调试信息，表示完全停止持续时间未达到阈值
    ADEBUG << "Current full stop lasting time(s) is "
           << is_full_stop_wait_time_diff_ << ", not reach the threshold: "
           << lon_based_pidcontroller_conf_.full_stop_long_time();
    return false;
  }
}
// 设置驻车制动器
void LonController::SetParkingBrake(const LonBasedPidControllerConf *conf,
                                    control::ControlCommand *control_command) {
  // 如果控制指令中驻车制动器开启
  if (control_command->parking_brake()) {
    // epb on, parking brake: 0 -> 1
    // 如果驻车制动器状态切换为开启，并且为第一阶段
    if (epb_on_change_switch_) {
      ADEBUG << "Epb on, first set parking brake false.";
      // 设置控制指令中的驻车制动器状态为关闭
      control_command->set_parking_brake(false);
      // 驻车制动器状态切换计数加一
      ++epb_change_count_;
      // 如果切换计数达到设定值
      if (epb_change_count_ >= conf->epb_change_count()) {
        // 关闭驻车制动器状态切换开关
        epb_on_change_switch_ = false;
        // 重置切换计数
        epb_change_count_ = 0;
        ADEBUG << "Epb on, first stage has been done.";
      }
    } else {
      ADEBUG << "Epb on, second set parking brake true.";
      // 设置控制指令中的驻车制动器状态为开启
      control_command->set_parking_brake(true);
      // 驻车制动器状态切换计数加一
      ++epb_change_count_;
      // 如果切换计数达到设定值
      if (epb_change_count_ >= conf->epb_change_count()) {
        // 打开驻车制动器状态切换开关
        epb_on_change_switch_ = true;
        // 重置切换计数
        epb_change_count_ = 0;
        ADEBUG << "Epb on, second stage has been done.";
      }
    }
  } else {
    // epb off, parking brake: 1 -> 0
    // 如果控制指令中驻车制动器关闭
    // 如果驻车制动器状态切换为关闭，并且为第一阶段
    if (epb_off_change_switch_) {
      ADEBUG << "Epb off, first set praking brake true.";
      // 设置控制指令中的驻车制动器状态为开启
      control_command->set_parking_brake(true);
      // 驻车制动器状态切换计数加一
      ++epb_change_count_;
      // 如果切换计数达到设定值
      if (epb_change_count_ >= conf->epb_change_count()) {
        epb_off_change_switch_ = false;// 关闭驻车制动器状态切换开关
        epb_change_count_ = 0;// 重置切换计数
        ADEBUG << "Epb off, first stage has been done.";
      }
    } else {
      ADEBUG << "Epb off, second set parking brake false.";
      // 设置控制指令中的驻车制动器状态为关闭
      control_command->set_parking_brake(false);
      // 驻车制动器状态切换计数加一
      ++epb_change_count_;
      // 如果切换计数达到设定值
      if (epb_change_count_ >= conf->epb_change_count()) {
        // 打开驻车制动器状态切换开关
        epb_off_change_switch_ = true;
        // 重置切换计数
        epb_change_count_ = 0;
        ADEBUG << "Epb off, second stage has been done.";
      }
    }
  }
}

}  // namespace control
}  // namespace apollo

3 纵向控制框架

看完上面的源码，对照着下图加深对纵向控制的理解

---

相信读完本文，读者会对百度Apollo的纵向控制有更深的理解