百度Apollo7.0_Control控制模块：纵向控制源码解析

Apollo中纵向控制主要靠纵向控制器类（LonController）来实现

纵向控制主要为速度控制，通过控制刹车、油门、档位等实现对车速的控制，对于自动挡车辆来说，控制对象其实就是刹车和油门。
以不加预估的控制为例，apollo纵向控制中计算纵向误差的原理：

其主要涉及到两个文件：
/apollo/modules/control/controller/lon_controller.h
/apollo/modules/control/controller/lon_controller.cc
现对上述两个文件源码解析如下：

（依照个人习惯对代码风格做了一些修改，包括有些为了梳理结构和阅读方便而存在的大长行，请自觉忽视不良习惯）

1.lon_controller.h

/******************************************************************************
 * 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.
 *****************************************************************************/

/**
 * @file
 * @brief Defines the LonController class.
 */

#pragma once

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

#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/filters/digital_filter.h"
#include "modules/common/filters/digital_filter_coefficients.h"
#include "modules/control/common/interpolation_2d.h"
#include "modules/control/common/leadlag_controller.h"
#include "modules/control/common/pid_controller.h"
#include "modules/control/common/trajectory_analyzer.h"
#include "modules/control/controller/controller.h"

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

/**
 * @class LonController
 *
 * @brief Longitudinal controller, to compute brake / throttle values.
 */
class LonController : public Controller {
 public:
  /**
   * @brief constructor
   */
  LonController();

  /**
   * @brief destructor
   */
  virtual ~LonController();

  /**
   * @brief initialize Longitudinal Controller
   * @param control_conf control configurations
   * 
   * @param injector  车辆当前状态信息
   * DependencyInjector类只有一个属性（vehicle_state_）和一个方法（获得vehicle_state_）
   * 
   * @return Status initialization status
   */
  common::Status Init(std::shared_ptr<DependencyInjector> injector,
                      const ControlConf *control_conf) 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
   */
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, 
      const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd) override;

  /**
   * @brief reset longitudinal controller
   * @return Status reset status
   */
  common::Status Reset() override;

  /**
   * @brief stop longitudinal controller
   */
  void Stop() override;

  /**
   * @brief longitudinal controller name
   * @return string controller name in string
   */
  std::string Name() const override;

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

    /**
     * @brief  Get the Path Remain object
     * 计算剩余路径（寻找轨迹点上接下来的第一个停车点）
     * @param debug 停车点信息存到debug指针里
     */
  void GetPathRemain(SimpleLongitudinalDebug *debug);

 private:
    /**
     * @brief Set the Digital Filter Pitch Angle object
     * 设置俯仰角数字滤波器
     * @param lon_controller_conf 
     * 参数：lon_controller_conf是纵向控制器配置类对象，
     * 该类由/apollo/modules/control/proto/lon_controller_conf.proto生成
     * 从该对象中读取截至频率，控制周期等参数来设置俯仰角滤波器
     * 
     * 俯仰角用来进行车辆的坡道补偿，默认坡道补偿是关闭的
     */
  void SetDigitalFilterPitchAngle(const LonControllerConf &lon_controller_conf);

    /**
     * @brief 加载控制标定表
     * 从纵向控制器配置对象中读取车速-加速度-控制百分数标定表
     * @param lon_controller_conf 
     */
  void LoadControlCalibrationTable(const LonControllerConf &lon_controller_conf);

    /**
     * @brief Set the Digital Filter object
     * 设置数字滤波器
     * @param ts 控制周期
     * @param cutoff_freq 截至频率
     * @param digital_filter 数字滤波器类对象，前面两项参数就是为了设置这个对象
     */
  void SetDigitalFilter(double ts, double cutoff_freq, common::DigitalFilter *digital_filter);

  void CloseLogFile();


  //定义成员常量指针localization_，存放的是定位来的信息，初始化为空指针
  const localization::LocalizationEstimate *localization_ = nullptr;
  //定义成员常量指针chassis_，存放的是来自canbus的车辆状态反馈信息，初始化为空指针
  const canbus::Chassis *chassis_ = nullptr;
 
  //定义成员二维插值点指针control_interpolation_，实际就是存放标定表及插值功能
  std::unique_ptr<Interpolation2D> control_interpolation_;
  //定义常量轨迹msg类指针trajectory_message_，是用来存放规划模块发来的轨迹消息
  const planning::ADCTrajectory *trajectory_message_ = nullptr;
  //定义轨迹分析者类对象trajectory_analyzer_，用来实现对各种轨迹信息的解析
  std::unique_ptr<TrajectoryAnalyzer> trajectory_analyzer_;
 
  //定义成员name_，就是控制器的名称
  std::string name_;
  //定义成员controller_initialized_，实际上就是表明控制器是否被初始化成功
  bool controller_initialized_ = false;
 
  //定义成员上一时刻的加速度
  double previous_acceleration_ = 0.0;
  //定义成员上一时刻的参考加速度
  double previous_acceleration_reference_ = 0.0;
 
  //定义PID控制器类对象speed_pid_controller_，纵向控制器里的速度控制器
  PIDController speed_pid_controller_;
  //定义PID控制器类对象station_pid_controller_，纵向控制器里的位置控制器
  PIDController station_pid_controller_;
 
  //纵向上leadlag控制器默认关闭
  //定义超前滞后控制器类对象 speed_leadlag_controller_，用于速度的控制
  LeadlagController speed_leadlag_controller_;
  //定义超前滞后控制器类对象 station_leadlag_controller_，用于位置的控制
  LeadlagController station_leadlag_controller_;
 
  //定义文件类对象 speed_log_file_用于存放纵向上的日志信息，默认也关闭csv日志
  FILE *speed_log_file_ = nullptr;
 
  //定义数字滤波器类对象digital_filter_pitch_angle_，用于对俯仰角pitch进行滤波
  common::DigitalFilter digital_filter_pitch_angle_;
  //定义常量成员控制配置类对象control_conf_指针，用于存放配置文件中加载进来的控制配置
  const ControlConf *control_conf_ = nullptr;
 
  // 定义成员车辆参数类对象，用于存放车辆的实际尺寸参数等
  common::VehicleParam vehicle_param_;
  //定义成员车辆状态信息类对象injector_,用于存放当前车辆的状态信息
  std::shared_ptr<DependencyInjector> injector_;
  
}; //class LonController
}  // namespace control
}  // namespace apollo

2.lon_controller.cc

/******************************************************************************
 * 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/lon_controller.h"

#include <algorithm>
#include <utility>

#include "absl/strings/str_cat.h"
#include "cyber/common/log.h"
#include "cyber/time/time.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/control/common/control_gflags.h"
#include "modules/localization/common/localization_gflags.h"

namespace apollo {
namespace control {

//使用了apollo/common的故障码，状态码，轨迹点，车辆状态信息类，还用到了apollo/cyber/Time类
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleStateProvider;
using apollo::cyber::Time;

// 定义常量重力加速度 9.8
constexpr double GRA_ACC = 9.8;

/*
  在FLAGS_取全局变量时，首先参考common文件夹下的gflags.h/gflags.cc文件的默认值；
  在模块启动和运行中，会参考dag文件依赖的目标。

  例如：
  此处首先取/apollo/modules/control/common/control_gflags.cc中的默认值，然后参考文
  件/apollo/modules/control/dag/lateral_longitudinal_module.dag中Line27-39部分，
  其中Line32中为 flag_file_path: "/apollo/modules/control/conf/control.conf"，
  则本文件中FLAGS_取的全局变量替换为"/apollo/modules/control/conf/control.conf"中的值；
  但上述文件中：
  Line1：flagfile=/apollo/modules/common/data/global_flagfile.txt
  则本文件中FLAGS_取的全局变量还要参考该文件中的内容。
  Line2：control_conf_file=/apollo/modules/control/conf/control_conf.pb.txt
  则本文件中FLAGS_control_conf_取的全局变量要参考该文件中的内容。

  其中Line31中为 config_file_path: "/apollo/modules/control/conf/control_common_conf.pb.txt"
  则本文件中 ControlConf control_conf_ 为"/apollo/modules/control/conf/control_common_conf.pb.txt"中的值
*/

/**
 * 关于FLAGS_所提取的全局变量：
 * ----------------------------------------------------------------------------------------------------------------------------
 * 变量名                | enable_csv_debug | enable_speed_station_preview | enable_slope_offset | use_preview_speed_for_table |
 *                      |                  |                              |                     |                             |
 * control_gflags.cc    |      false       |            true              |        false        |             false           |
 *                      |                  |                              |                     |                             |
 * control.conf         |        -         |            false             |        false        |             false           |
 *                      |                  |                              |                     |                             |
 * global_flagfile.txt  |        -         |              -               |          -          |               -             |
 *                      |                  |                              |                     |                             |
 * 使用值                |      false       |            false             |        false        |             false           |
 * ----------------------------------------------------------------------------------------------------------------------------
 * 
 */


// ### Line40-80
// #1 构造函数
LonController::LonController(): name_(ControlConf_ControllerType_Name(ControlConf::LON_CONTROLLER)) 
{
  // FLAGS_enable_csv_debug = false(default)
  if (FLAGS_enable_csv_debug) 
  {
    //如果打开就是创建一个以时间命名的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,"
              "acceleration_cmd,"
              "acceleration_lookup,"
              "speed_lookup,"
              "calibration_value,"
              "throttle_cmd,"
              "brake_cmd,"
              "is_full_stop,"
              "\r\n");

      fflush(speed_log_file_);
    }

    AINFO << name_ << " used.";
  }
}


// ### Line82-92
// #2 关闭log日志文件
// 被 #3 Stop()函数、#4 析构函数调用
void LonController::CloseLogFile() 
{
  // FLAGS_enable_csv_debug = false(default)
  if (FLAGS_enable_csv_debug) 
  {
    if (speed_log_file_ != nullptr) 
    {
      fclose(speed_log_file_);
      speed_log_file_ = nullptr;
    }
  }
}

// #3 Stop()函数
// 调用#2 CloseLogFile()关闭日志
void LonController::Stop() { CloseLogFile(); }

// #4 析构函数
// 调用#2 CloseLogFile()关闭日志
LonController::~LonController() { CloseLogFile(); }


// ### Line94-129
/**
 * @brief 纵向控制器的初始化函数
 * 
 * @param injector DependencyInjector类对象指针injector主要是用来获取车辆状态信息的
 * @param control_conf 控制的配置类对象指针
 * @return Status 返回初始化状态
 */
// #5 初始化函数
// 1）读取车辆状态信息和车辆控制配置信息
// 2）初始化位置和速度PID控制器，若打开超前滞后控制器开关则也初始化位置和速度的超前滞后控制器
// 3）拷贝车辆参数到vehicle_param_
// 4）调用 #6 SetDigitalFilterPitchAngle() 加载俯仰角滤波器参数
// 5）调用 #7 LoadControlCalibrationTable() 加载标定表
Status LonController::Init(std::shared_ptr<DependencyInjector> injector, const ControlConf *control_conf) 
{
  //将传进来的车辆配置信息control_conf赋值给纵向控制器成员control_conf_
  control_conf_ = control_conf;
  //如果加载的配置为空，则直接返回错误信息
  if (control_conf_ == nullptr) 
  {
    //控制器初始化标志位
    controller_initialized_ = false;
    AERROR << "get_longitudinal_param() nullptr";
    return Status(ErrorCode::CONTROL_INIT_ERROR,
                  "Failed to load LonController conf");
  }

  //将传进来的车辆状态injector赋值给纵向控制器成员injector_
  injector_ = injector;
  //此处取车辆配置信息的纵向控制部分的参数给到lon_controller_conf
  //即取/apollo/modules/control/conf/control_conf.pb.txt中lon_controller_conf的内容（L124以后内容）
  const LonControllerConf &lon_controller_conf = control_conf_->lon_controller_conf();
  //控制周期ts(default = 0.01s, in control_conf.pb.txt)
  double ts = lon_controller_conf.ts();
  //超前滞后控制器开关(default = false, in control_conf.pb.txt)
  bool enable_leadlag = lon_controller_conf.enable_reverse_leadlag_compensation();

  // 位置PID控制器的初始化
  station_pid_controller_.Init(lon_controller_conf.station_pid_conf());
  /**
   * in control_conf.pb.txt, Line133-139
   * station_pid_conf {
   *   integrator_enable: false
   *   integrator_saturation_level: 0.3
   *   kp: 0.2
   *   ki: 0.0
   *   kd: 0.0
   * }
   */
  // 速度PID控制器的初始化
  speed_pid_controller_.Init(lon_controller_conf.low_speed_pid_conf());
    /**
   * in control_conf.pb.txt, Line140-146
   * low_speed_pid_conf {
   *   integrator_enable: true
   *   integrator_saturation_level: 0.3
   *   kp: 2.0
   *   ki: 0.3
   *   kd: 0.0
   * }
   */
  
  //(default = false, in control_conf.pb.txt)
  if (enable_leadlag) 
  {
    //in control_conf.pb.txt, Line170-181
    station_leadlag_controller_.Init(lon_controller_conf.reverse_station_leadlag_conf(), ts);
    speed_leadlag_controller_.Init(lon_controller_conf.reverse_speed_leadlag_conf(), ts);
  }


  // 拷贝车辆参数配置到vehicle_param_(LonController类的数据成员)
  // 类的单例模式【待学习】在/apollo/modules/common/configs/vehicle_config_helper.h中Line123定义DECLARE_SINGLETON(VehicleConfigHelper)
  // 使用VehicleConfigHelper类的成员函数GetConfig()加载，即/apollo/modules/common/configs/vehicle_config_helper.cc中Line47-52
  //    调用Init，即Line33，Init() { Init(FLAGS_vehicle_config_path); }
  // 其中vehicle_config_path在 /apollo/modules/common/configs/config_gflags.cc中Line40定义为：
  //    /apollo/modules/common/data/vehicle_param.pb.txt，即取该文件中的车辆参数
  // 上述文件（vehicle_param.pb.txt）在/apollo/modules/common/data/global_flagfile.txt中Line8定义其具体车辆模型，例如在
  //   /apollo/modules/calibration/data/***下定义具体车辆模型参数后，通过global_flagfile.txt中Line8选择对应模型即可
  vehicle_param_.CopyFrom(common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());

  //从modules/control/conf/control_conf.pb.txt->lon_controller_conf里加载数字滤波器的参数
  //加载滤波器参数到LonController类的成员变量digital_filter_pitch_angle_
  //这个滤波器是用于对pitch角(车辆的俯仰角)进行滤波，pitch角是用于对车辆控制时的坡道补偿
  SetDigitalFilterPitchAngle(lon_controller_conf);

  //从modules/control/conf/control_conf.pb.txt->lon_controller_conf里加载标定表(不同车速下，加速度到油门/制动标定表)
  //加载的标定表放入control_interpolation_中
  LoadControlCalibrationTable(lon_controller_conf);
	
  //执行到这里，纵向控制器被初始化的标志位置为true，这个标志位也属于LonController类的数据成员
  controller_initialized_ = true;

  return Status::OK();
}

// ### Line131-137
// #6 加载俯仰角滤波器参数
// 被 #5 Init()调用； 调用#12 SetDigitalFilter()
void LonController::SetDigitalFilterPitchAngle(const LonControllerConf &lon_controller_conf) 
{
  // 截止频率(default = 5, in control_conf.pb.txt)
  double cutoff_freq = lon_controller_conf.pitch_angle_filter_conf().cutoff_freq();
  //采样周期(default = 0.01s, in control_conf.pb.txt)
  double ts = lon_controller_conf.ts();
  //将ts，cutoff_freq作为输入参数，调用函数 #12 SetDigitalFilter()
  //将参数全部设置到LonController类的数据成员滤波器类对象digital_filter_pitch_angle_中
  SetDigitalFilter(ts, cutoff_freq, &digital_filter_pitch_angle_);
}

// ### Line139-154
// #7 加载标定表
// 被 #5 Init()调用
void LonController::LoadControlCalibrationTable(const LonControllerConf &lon_controller_conf) 
{
  //首先将从lon_controller_conf中读到的标定表calibration_table()存入control_table中
  //加const修饰防止引用变量别名更改原变量
  //in control_conf.pb.txt Line185-6671
  const auto &control_table = lon_controller_conf.calibration_table();
  //屏幕上打印标定表成功加载信息
  AINFO << "Control calibration table loaded";
  AINFO << "Control calibration table size is " << control_table.calibration_size();
  
  // 定义DataType类型变量xyz
  // Apollo类型定义typedef std::vector<std::tuple<double, double, double>> DataType;
  // DataType实际上就是一个三维容器，存放很多组对应的速度，加速度，油门/制动百分数数据
  Interpolation2D::DataType xyz;
  for (const auto &calibration : control_table.calibration()) 
  {
    //依次control_table下的calibration读入速度，加速度，油门/制动百分数数据压入xyz中
    xyz.push_back(std::make_tuple(calibration.speed(),
                            calibration.acceleration(),
                            calibration.command()));
  }

  //std::unique_ptr指针的清空复位，将LonController类数据成员control_interpolation_清空
  control_interpolation_.reset(new Interpolation2D);
  //control_interpolation_调用Init函数,xyz作为参数输入，
  //将标定表读取到LonController类的数据成员xyz_里(特定车速下的加速度对应的控制量百分数)
  ACHECK(control_interpolation_->Init(xyz))
      << "Fail to load control calibration table";
}

// ### Line156-369
// #8 计算纵向控制指令
/**
 * @brief 计算纵向控制指令：根据当前车辆状态和目标轨迹计算制动/油门值
 * 
 * @param localization 定位信息指针
 * @param chassis 底盘信息指针
 * @param planning_published_trajectory 规划轨迹信息指针
 * @param cmd 存放计算出的控制命令
 * @return Status 计算结果状态
 */
//ControlCommand类是由modules/control/proto/control_cmd.proto里定义的message ControlCommand定义
Status LonController::ComputeControlCommand(
                      const localization::LocalizationEstimate *localization,
                      const canbus::Chassis *chassis,
                      const planning::ADCTrajectory *planning_published_trajectory,
                      control::ControlCommand *cmd) 
{
  localization_ = localization;
  chassis_ = chassis;
  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的序号不相等， 则复位重置
	//trajectory_analyzer_是LonController类的数据成员，属于类TrajectoryAnalyzer在/apollo/modules/control/common/trajectory_analyzer.h下定义
	//trajectory_message_也是LonController类数据成员，属于类ADCTrajectory由apollo/modules/planning/proto/planning.proto定义的消息类
  if (trajectory_analyzer_ == nullptr ||
      trajectory_analyzer_->seq_num() != trajectory_message_->header().sequence_num()) 
  {
    trajectory_analyzer_.reset(new TrajectoryAnalyzer(trajectory_message_));
  }

  const LonControllerConf &lon_controller_conf = control_conf_->lon_controller_conf();

  //定义临时变量debug是cmd.debug.simple_lon_debug ，然后清空
  auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
  debug->Clear();

  double brake_cmd = 0.0;
  double throttle_cmd = 0.0;
  //(default = 0.01s, in control_conf.pb.txt)
  double ts = lon_controller_conf.ts();
  //预览时间 = lon_controller_conf读到的纵向预览窗口大小*采样时间ts
  //(default = 20.0, in control_conf.pb.txt)
  double preview_time = lon_controller_conf.preview_window() * ts;
  //超前-滞后控制器开关(default = false, in control_conf.pb.txt)
  bool enable_leadlag = lon_controller_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);
  }

  //调用 #11 计算纵向误差函数
  //trajectory_analyzer_.get()获得轨迹信息指针用于提供轨迹点的速度、加速度，匹配点、参考点等信息
  //计算得到的误差放入debug中，纵向误差计算的细节在下面函数定义时再详细介绍
  ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts, debug);

  // 纵向位置误差限制量(default = 2.0, in control_conf.pb.txt)
  double station_error_limit = lon_controller_conf.station_error_limit();
  // 经限幅后的纵向位置误差
  double station_error_limited = 0.0;

  // 对纵向位置误差进行限值
  /**
   * //基本概念 预览点（preview_point）：当前时间加上预览时间在轨迹上对应的点，车辆将要到达的纵向位置，参见本文档Line736（源码文件Line407）
   * //基本概念 参考点（reference_point）：当前时间车辆在轨迹上应该到达的点，就是用当前时间去轨迹上查找最近的点，参见本文档Line734（源码文件Line404）
   * //基本概念 匹配点（matched_point）：当前车辆位置在轨迹上对应的点，就是将当前车辆坐标带入轨迹中找到的最近点，参见本文档Line711（源码文件Line393）
   * // preview_station_error = 预览点纵向位置 - 匹配点纵向位置
   * // station_error = 参考点纵向位置-匹配点纵向位置
   * 
   */
  // FLAGS_enable_speed_station_preview = false(default)
  if (FLAGS_enable_speed_station_preview) 
  {
    // Clamp（x, a, b）上下幅值限制，Clamp（x, a, b）= x∈[a, b]
    station_error_limited = common::math::Clamp(
                            debug->preview_station_error(),
                            -station_error_limit, 
                            station_error_limit);
  } 
  else 
  {
    station_error_limited = common::math::Clamp(
                            debug->station_error(), 
                            -station_error_limit, 
                            station_error_limit);
  }

  //设置PID控制器参数
  /**
   * //如果轨迹msg信息指针里的档位信息是R档，则从配置文件加载lon_controller_conf里的倒车PID参数配置加载到
   *   位置PID控制器对象station_pid_controller_ 和 速度PID控制器对象speed_pid_controller_
   *   若打开超前之后控制器，设置对应的 位置 和 速度 控制参数
   * 
   * //如果非R档 且 当前车速 <= 高低速切换的转换速度，则速度PID控制器对象speed_pid_controller_加载控制配置文件中低速PID参数
   * 
   * //否则速度PID控制器对象speed_pid_controller_加载控制配置文件中高速PID参数（通常低速PID参数要更大些）
   * 
   */
  if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) 
  {
    //in control_conf.pb.txt, Line156-169
    station_pid_controller_.SetPID(lon_controller_conf.reverse_station_pid_conf());
    speed_pid_controller_.SetPID(lon_controller_conf.reverse_speed_pid_conf());
    if (enable_leadlag) 
    {
      //in control_conf.pb.txt, Line170-181
      station_leadlag_controller_.SetLeadlag(lon_controller_conf.reverse_station_leadlag_conf());
      speed_leadlag_controller_.SetLeadlag(lon_controller_conf.reverse_speed_leadlag_conf());
    }
  } 
  //lon_controller_conf.switch_speed() = 3.0, in control_conf.pb.txt
  else if (injector_->vehicle_state()->linear_velocity() <= lon_controller_conf.switch_speed()) 
  {
    //in control_conf.pb.txt, Line140-146
    speed_pid_controller_.SetPID(lon_controller_conf.low_speed_pid_conf());
  } 
  else 
  {
    //in control_conf.pb.txt, Line147-153
    speed_pid_controller_.SetPID(lon_controller_conf.high_speed_pid_conf());
  }

  //定义临时变量速度偏差（speed_offset）= 位置控制器根据(限幅后位置误差，采样周期)计算出控制量即速度
  //Control()是PID控制器的成员函数，根据PID控制器原理计算控制量速度作为speed_offset
  double speed_offset = station_pid_controller_.Control(station_error_limited, ts);
  //若打开超前之后控制器，使用超前滞后控制器更新计算出的控制量
  if (enable_leadlag) 
  {
    speed_offset = station_leadlag_controller_.Control(speed_offset, ts);
  }

  //速度控制器的输入
  double speed_controller_input = 0.0;
  //从配置文件加载速度控制器输入限幅(default = 2.0, in control_conf.pb.txt)
  double speed_controller_input_limit = lon_controller_conf.speed_controller_input_limit();
  //经过限幅之后的速度控制器的输入
  double speed_controller_input_limited = 0.0;
  //若打开速度-位置预览开关，则速度控制器的输入 = 位置控制器计算出的speed_offset + 预览点的速度和当前车速的偏差
  //否则，速度控制器的输入 = 位置控制器计算出的speed_offset + 参考点的速度和当前车速的偏差
  // FLAGS_enable_speed_station_preview = false(default)
  if (FLAGS_enable_speed_station_preview) 
  {
    speed_controller_input = speed_offset + debug->preview_speed_error();
  } 
  else 
  {
    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);
  
  //闭环加速度指令，就是速度PID控制器根据输入的经限幅的控制器输入量 与 采样时间计算的结果
  double acceleration_cmd_closeloop = 0.0;
  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());
  //若打开超前之后控制器，则超前滞后控制器更新计算出的闭环加速度指令 和 PID控制器中积分器的饱和状态
  if (enable_leadlag) 
  {
    acceleration_cmd_closeloop = speed_leadlag_controller_.Control(acceleration_cmd_closeloop, ts);
    debug->set_leadlag_saturation_status(speed_leadlag_controller_.InnerstateSaturationStatus());
  }
  //斜坡补偿加速度 = (g * sinθ)再经过数字滤波器滤波得到斜坡加速度补偿
  double slope_offset_compensation = digital_filter_pitch_angle_.Filter(
      GRA_ACC * std::sin(injector_->vehicle_state()->pitch()));
  //判断坡道补偿加速度是否为非数NaN，当浮点数过小下溢就可能出现NaN非数
  if (std::isnan(slope_offset_compensation)) 
  {
    slope_offset_compensation = 0;
  }
  //将斜坡补偿加速度设置到debug里
  debug->set_slope_offset_compensation(slope_offset_compensation);
  //总的加速度指令 = 闭环加速度指令 + 预览参考加速度 + 坡道补偿加速度(如果打开坡道补偿的话)
  // FLAGS_enable_slope_offset = false(default)
  double acceleration_cmd = acceleration_cmd_closeloop + 
                            debug->preview_acceleration_reference() +
                            FLAGS_enable_slope_offset * debug->slope_offset_compensation();
  
  //设置是否完全停车标志位
  debug->set_is_full_stop(false);
  //获取停车点，调用 #13 GetPathRemain() 函数。
  //找到当前规划模块发布的轨迹msg里的第一个v,a都小于阈值的点作为停车点
	//找到的这个停车点的纵向位置和当前车辆纵向位置的偏差设置到debug里面去，debug.path_remain()
  GetPathRemain(debug);

  // At near-stop stage, replace the brake control command with the standstill acceleration if the former is even softer than the latter
  // 在接近停止阶段，如果制动控制命令比静止加速度更软，则将制动控制命令替换为静止加速度，就是用一个固定的standstill的减速度代替刹车控制指令
  // 简而言之就是快到停车点时给一个固定-0.3m/s^2的减速度(数值控制配置里设置)

  // trajectory_message_是由apollo/modules/planning/proto/planning.proto中定义的ADCTrajectory类对象，其中
  // trajectory_type为一个枚举点轨迹类型，默认为 UNKNOWN，还有：NORMAL(估计应该是向前的正常轨迹)， PATH_FALLBACK， SPEED_FALLBACK， PATH_REUSED

  /**
   * //   若( 轨迹类型为NORMAL 且 
   * //       ( (预览点的加速度 <= 控制配置里的停车时最大允许加速度 且 
   * //                预览点速度 <= 车辆参数中的最大允许停车速度) 或 
   * //          当前纵向位置到停止点纵向位置偏差小于控制配置里停车时最大允许的距离))
   * 
   * 按照dag文件，此处control_conf_所取的参数应该是文件control_common_conf.pb.txt中的？？？
   * // max_acceleration_when_stopped 在/apollo/modules/control/conf/control_common_conf.pb.txt中为0.01
   * // max_path_remain_when_stopped 在/apollo/modules/control/conf/control_common_conf.pb.txt中为0.3
   * 在/apollo/modules/control/conf/control_conf.pb.txt中，上述二者也分别为：0.01 和 0.3
   */
  if ((trajectory_message_->trajectory_type() == apollo::planning::ADCTrajectory::NORMAL)  &&
      ((std::fabs(debug->preview_acceleration_reference()) <= control_conf_->max_acceleration_when_stopped() 
            && std::fabs(debug->preview_speed_reference()) <= vehicle_param_.max_abs_speed_when_stopped()) ||
        std::abs(debug->path_remain()) < control_conf_->max_path_remain_when_stopped())) 
  {
    // 若： 轨迹类型NORMAL 且 ((预览点加速度小于阈值 且 预览点速度小于阈值) 或 到停车点纵向偏差<阈值)
    // 则： 若R档则取二者最大值，否则取二者最小值 (其中二者分别为计算出的加速度控制指令 和 控制配置的standstill)
    // lon_controller_conf.standstill_acceleration() = -0.3, in control_conf.pb.txt
    acceleration_cmd = (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
                        ? std::max(acceleration_cmd, -lon_controller_conf.standstill_acceleration())
                        : std::min(acceleration_cmd, lon_controller_conf.standstill_acceleration());

    ADEBUG << "Stop location reached";
    //若正常执行到这里，车辆就已经完全停住了，将是否完全停车标志位设置为true
    debug->set_is_full_stop(true);
  }

  //定义油门指令的下边界，为 车辆配置里的throttle_deadzone 和 lon_controller_conf配置里的throttle_minimum_action两者中的较大值
  // lon_controller_conf.throttle_minimum_action() = 0.0, in control_conf.pb.txt
  double throttle_lowerbound = std::max(vehicle_param_.throttle_deadzone(), lon_controller_conf.throttle_minimum_action());
  //定义刹车指令的下边界，为 车辆配置里的brake_deadzone 和 lon_controller_conf配置里的brake_minimum_action两者中的较大值
  // lon_controller_conf.brake_minimum_action() = 0.0, in control_conf.pb.txt
  double brake_lowerbound = std::max(vehicle_param_.brake_deadzone(), lon_controller_conf.brake_minimum_action());
  //定义查表得到的标定值（控制百分数值）并初始化为0
  double calibration_value = 0.0;
  //用于查标定表的请求加速度，R档时取加速度指令值的负值，否则取加速度指令值
  double acceleration_lookup = (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE) ? -acceleration_cmd : acceleration_cmd;

  //用预览点速度来查标定表开关
  // FLAGS_use_preview_speed_for_table = false(default)
  if (FLAGS_use_preview_speed_for_table) 
  // use_preview_speed_for_table 在/apollo/modules/control/common/control_gflags.cc中为false
  {
    //二维插值点指针control_interpolation_
    //若打开开关，则使用 预览点速度 和 用于查标定表的请求加速度 来插值获得 查表得到的标定值
    calibration_value = control_interpolation_->Interpolate(std::make_pair(debug->preview_speed_reference(), acceleration_lookup));
  } 
  else 
  {
    //若关闭开关，则使用 实际车辆速度 和 用于查标定表的请求加速度 来插值获得 查表得到的标定值
    calibration_value = control_interpolation_->Interpolate(std::make_pair(chassis_->speed_mps(), acceleration_lookup));
  }

  //计算制动指令 和 加速指令
  if (acceleration_lookup >= 0) 
  {
    if (calibration_value >= 0) 
    {
      //若 用于查标定表的请求加速度非负 且 查表得到的标定值非负， 则 油门控制指令 取 标定值 和 油门指令的下边界 的最大值
      throttle_cmd = std::max(calibration_value, throttle_lowerbound);
    } 
    else 
    {
      //若 用于查标定表的请求加速度非负 且 查表得到的标定值为负， 则 油门控制指令 取 油门指令的下边界值
      throttle_cmd = throttle_lowerbound;
    }
    //若 用于查标定表的请求加速度非负，则制动控制指令取0
    brake_cmd = 0.0;
  } 
  else 
  {
    //若 用于查标定表的请求加速度为负，则油门控制指令取0
    throttle_cmd = 0.0;
    if (calibration_value >= 0) 
    {
      //若 用于查标定表的请求加速度为负 且 查表得到的标定值非负， 则 制动控制指令 取 制动指令的下边界值
      brake_cmd = brake_lowerbound;
    } 
    else 
    {
      //若 用于查标定表的请求加速度为负 且 查表得到的标定值为负， 则 制动控制指令 取 标定值的负数 和 制动指令的下边界 的最大值
      brake_cmd = std::max(-calibration_value, brake_lowerbound);
    }
  }

  //设置debug信息
  //被限制的纵向位置误差
  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_speed_lookup(chassis_->speed_mps());
  //查表得到的标定值（控制百分数值）
  debug->set_calibration_value(calibration_value);
  //闭环反馈速度控制器计算得到的控制量加速度
  debug->set_acceleration_cmd_closeloop(acceleration_cmd_closeloop);

  //如果打开csv日志记录（默认false） 且  速度日志文件不空，则写日志
  // FLAGS_enable_csv_debug = false(default)
  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->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);
  cmd->set_acceleration(acceleration_cmd);

  //（纵向速度绝对值 <= 停车状态的最大值）  或   N档
  if (std::fabs(injector_->vehicle_state()->linear_velocity()) <= vehicle_param_.max_abs_speed_when_stopped()
      || chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) 
  //车辆的纵向速度绝对值小于某阈值或者chassis反馈的档为信息是N档就认为车已经停住了
  {
    //若车辆处于停车或N档时 下发规划发布的轨迹msg里的档位
    cmd->set_gear_location(trajectory_message_->gear());
  } 
  else 
  {
    //若车辆不处于停车且不在N档时 下发chassis反馈的车辆实际档位
    cmd->set_gear_location(chassis->gear_location());
  }

  return Status::OK();
}

// ### Line371-375
// #9 重置
// 重置速度PID控制器 和 位置PID控制器， 并返回重置状态
Status LonController::Reset() 
{
  speed_pid_controller_.Reset();
  station_pid_controller_.Reset();
  return Status::OK();
}

// ### Line377
// #10 控制器名称
// 在lon_controller.cc Line41 LonController类的构造函数初始化时就初始化name_为"LON_CONTROLLER"
std::string LonController::Name() const { return name_; }

// ### Line379-461
// #11 计算纵向误差
// 被 #8 ComputeControlCommand()调用
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

	  分解车辆运动到Frenet坐标，就是分为纵向和横向
	  s: 纵向累积走过的距离沿着参考轨迹
	  s_dot: 纵向沿着参考轨迹的速度
	  d: 相对参考轨迹的横向距离
	  d_dot: 横向距离的变化率
	  matched:匹配点，在参考轨迹上距离当前车辆距离最近的点
	  初始化匹配点处的s,s',d,d'
  */
  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();
  //匹配点为将车辆当前状态的x,y坐标代入去查找轨迹上的最近点
  //函数QueryMatchedPathPoint()在apollo/modules/control/common/trajectory_analyzer.cc中定义
  auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(vehicle_state->x(), vehicle_state->y());

  //函数ToTrajectoryFrame()在apollo/modules/control/common/trajectory_analyzer.cc中定义
  //轨迹信息将  当前点x,y,theta,v  以及  参考点信息输入，输出当前点的s,d,s',d'
  //简而言之就是将大地坐标系转化为Frenet坐标
  //d是横向偏差，s是累积的弧长即纵向上走过的距离
  //函数参数最后几个都带&，熟悉的套路，引用变量传值，最后带&的几个变量都是待填充函数结果的变量，即输出
  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 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这个指针对象里
  //匹配点x
  debug->mutable_current_matched_point()->mutable_path_point()->set_x(matched_point.x());
  //匹配点y
  debug->mutable_current_matched_point()->mutable_path_point()->set_y(matched_point.y());
  //参考点x
  debug->mutable_current_reference_point()->mutable_path_point()->set_x(reference_point.path_point().x());
  //参考点y
  debug->mutable_current_reference_point()->mutable_path_point()->set_y(reference_point.path_point().y());
  //预览点x
  debug->mutable_preview_reference_point()->mutable_path_point()->set_x(preview_point.path_point().x());
  //预览点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);
  //貌似是将大地坐标系转化到Frenet坐标纵向上引入的？？？？？？？？？
  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.current_speed = lon_speed
  debug->set_current_speed(lon_speed);
  //速度误差就是参考点速度减匹配点速度 debug.speed_error
  debug->set_speed_error(reference_point.v() - s_dot_matched);
  //加速度参考就是参考点的加速度存到debug里
  debug->set_acceleration_reference(reference_point.a());
  //纵向加速度存到debug里
  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;
  //参考加加速度存到debug
  debug->set_jerk_reference(jerk_reference);
  //当前的加加速度存到debug
  debug->set_current_jerk(lon_jerk);
  //加加速度误差 = 加加速度参考 - 纵向加加速度 / (1-kd) 。
  debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
  //将此刻的参考加速度赋给上一时刻参考加速度进行迭代
  previous_acceleration_reference_ = debug->acceleration_reference();
  //将当前的加速度赋给上一时刻的加速度，这样迭代一般都是为了计算差分来近似导数
  previous_acceleration_ = debug->current_acceleration();

  //预览点位置误差 = 预览点的纵向位置s - 匹配点纵向位置s
  debug->set_preview_station_error(preview_point.path_point().s() - s_matched);
  //预览点速度误差 = 预览点的纵向速度v - 匹配点纵向速度 
  debug->set_preview_speed_error(preview_point.v() - s_dot_matched);
  //预览点速度设置到debug中
  debug->set_preview_speed_reference(preview_point.v());
  //预览点加速度设置到debug中
  debug->set_preview_acceleration_reference(preview_point.a());

}

// ### Line463-469
// #12 设置数字滤波器
// 被#6 SetDigitalFilterPitchAngle()调用
void LonController::SetDigitalFilter(double ts, double cutoff_freq, common::DigitalFilter *digital_filter) 
{
  std::vector<double> denominators;
  std::vector<double> numerators;
  //在/apollo/modules/common/filters/digital_filter_coefficients.cc中定义的二阶巴特沃斯低通滤波器
  common::LpfCoefficients(ts, cutoff_freq, &denominators, &numerators);
  digital_filter->set_coefficients(denominators, numerators);
}

// ### Line471-514
// TODO(all): Refactor and simplify
// #13 获取停车点
// 被 #8 ComputeControlCommand()调用
/**
 * @brief 获取停车点
 * 在#8 ComputeControlCommand()函数中调用了该函数，目的就是先找到最新发布的轨迹上的第一个停车点，
 * 然后快到停车点时，就给一个固定的standstill减速度
 * 
 * @param debug debug指针，就是去轨迹点上找第一个符合条件的停止点，并将结果存在debug指针
 */
void LonController::GetPathRemain(SimpleLongitudinalDebug *debug) 
{
  //停车点在轨迹中的下标id
  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;

  if (trajectory_message_->gear() == canbus::Chassis::GEAR_DRIVE) 
  {
    //前进档，遍历当前轨迹msg中各个点是否满足停车条件，stop_index从0递增
    //若找到符合条件的则停止遍历，否则循环遍历，条件即上述定义的静态常量阈值   
    //_size()用于访问message类下repeated数组成员的长度
    while (stop_index < trajectory_message_->trajectory_point_size()) 
    {
      auto &current_trajectory_point = trajectory_message_->trajectory_point(stop_index);
      //前进档时，若某点处: v < 阈值(kSpeedThreshold) 且 阈值(kForwardAccThreshold) < a < 0;则认为是停车点
      if (fabs(current_trajectory_point.v()) < kSpeedThreshold &&
          current_trajectory_point.a() > kForwardAccThreshold &&
          current_trajectory_point.a() < 0.0) 
      {
        break;
      }
      ++stop_index;
    }
  } 
  else 
  {
    //非前进档，遍历当前轨迹msg中各个点是否满足停车条件，stop_index从0递增
    while (stop_index < trajectory_message_->trajectory_point_size()) 
    {
      auto &current_trajectory_point = trajectory_message_->trajectory_point(stop_index);
      //非前进档时，若某点处: v < 阈值(kSpeedThreshold) 且 0 < a < 阈值(kBackwardAccThreshold);则认为是停车点
      if (current_trajectory_point.v() < kSpeedThreshold &&
          current_trajectory_point.a() < kBackwardAccThreshold &&
          current_trajectory_point.a() > 0.0) 
      {
        break;
      }
      ++stop_index;
    }
  }

  //若找到轨迹的最后一个点仍不符合上述停车点的条件，则将轨迹的最后一个点作为停车点使用
  if (stop_index == trajectory_message_->trajectory_point_size()) 
  {
    //数组长度 -1 则为数组最后一位的下标id，故此处有 -1
    --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());
}

}  // namespace control
}  // namespace apollo

其中有些内容未作详细解释，若有疑问可留言讨论，如有误请留言指出，感谢！

本文参考：Apollo control模块纵向控制原理及核心代码逐行解析
【运动控制】Apollo6.0的lon_controller解析
Apollo代码学习(五)—横纵向控制