apollo7.0——规划代码解析

Planning模块的入口为"planning_component.h"和"planning_component.cc"两个文件，实现的功能如下：

bool PlanningComponent::Init() {
  injector_ = std::make_shared<DependencyInjector>();

  /**
   * modules/common/configs/config_gflags.cc
   *
   * DEFINE_bool(use_navigation_mode, false,
   *         "Use relative position in navigation mode");
   * **/
  if (FLAGS_use_navigation_mode) {
    // 导航规划，主要的应用场景是开放道路的自动驾驶。
    planning_base_ = std::make_unique<NaviPlanning>(injector_);
  } else {
    // 车道规划
    planning_base_ = std::make_unique<OnLanePlanning>(injector_);
  }

  /**
   * 该模块对应的配置文件
   * modules/planning/conf/planning_config.pb.txt
   * **/
  ACHECK(ComponentBase::GetProtoConfig(&config_))<< "failed to load planning config file "<< ComponentBase::ConfigFilePath();

  /**
   * modules/planning/common/planning_gflags.cc
   *
   * DEFINE_bool(planning_offline_learning, false,
   *         "offline learning. read record files and dump learning_data");
   *
   * learning_mode: NO_LEARNING
   * **/
  if (FLAGS_planning_offline_learning ||
      config_.learning_mode() != PlanningConfig::NO_LEARNING) {
    if (!message_process_.Init(config_, injector_)) {
      AERROR << "failed to init MessageProcess";
      return false;
    }
  }

  planning_base_->Init(config_);

  //  routing_response_topic: "/apollo/routing_response"
  routing_reader_ = node_->CreateReader<RoutingResponse>(
      config_.topic_config().routing_response_topic(),
      [this](const std::shared_ptr<RoutingResponse>& routing) {
        AINFO << "Received routing data: run routing callback."
              << routing->header().DebugString();
        std::lock_guard<std::mutex> lock(mutex_);
        routing_.CopyFrom(*routing);
      });

  //  traffic_light_detection_topic: "/apollo/perception/traffic_light"
  traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
      config_.topic_config().traffic_light_detection_topic(),
      [this](const std::shared_ptr<TrafficLightDetection>& traffic_light) {
        ADEBUG << "Received traffic light data: run traffic light callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        traffic_light_.CopyFrom(*traffic_light);
      });

  //  planning_pad_topic: "/apollo/planning/pad"
  pad_msg_reader_ = node_->CreateReader<PadMessage>(
      config_.topic_config().planning_pad_topic(),
      [this](const std::shared_ptr<PadMessage>& pad_msg) {
        ADEBUG << "Received pad data: run pad callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        pad_msg_.CopyFrom(*pad_msg);
      });

  // story_telling_topic: "/apollo/storytelling"
  story_telling_reader_ = node_->CreateReader<Stories>(
      config_.topic_config().story_telling_topic(),
      [this](const std::shared_ptr<Stories>& stories) {
        ADEBUG << "Received story_telling data: run story_telling callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        stories_.CopyFrom(*stories);
      });

  /**
   * modules/common/configs/config_gflags.cc
   *
   * DEFINE_bool(use_navigation_mode, false,
   *         "Use relative position in navigation mode");
   * **/
  if (FLAGS_use_navigation_mode) {
    relative_map_reader_ = node_->CreateReader<MapMsg>(
        config_.topic_config().relative_map_topic(),
        [this](const std::shared_ptr<MapMsg>& map_message) {
          ADEBUG << "Received relative map data: run relative map callback.";
          std::lock_guard<std::mutex> lock(mutex_);
          relative_map_.CopyFrom(*map_message);
        });
  }

  // planning_trajectory_topic: "/apollo/planning"
  planning_writer_ = node_->CreateWriter<ADCTrajectory>(config_.topic_config().planning_trajectory_topic());

  // routing_request_topic: "/apollo/routing_request"
  rerouting_writer_ = node_->CreateWriter<RoutingRequest>(config_.topic_config().routing_request_topic());

  // planning_learning_data_topic: "/apollo/planning/learning_data"
  planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(config_.topic_config().planning_learning_data_topic());

  return true;
}

Proc

bool PlanningComponent::Proc(
    const std::shared_ptr<prediction::PredictionObstacles>&prediction_obstacles,
    const std::shared_ptr<canbus::Chassis>& chassis,
    const std::shared_ptr<localization::LocalizationEstimate>&localization_estimate) {
  ACHECK(prediction_obstacles != nullptr);

  // check and process possible rerouting request
  /**
   * 检查是否需要重新规划线路。
   * **/
  // 步骤1 
  CheckRerouting();

  // process fused input data

  /**
   * struct LocalView {
   * std::shared_ptr<prediction::PredictionObstacles> prediction_obstacles;
   * std::shared_ptr<canbus::Chassis> chassis;
   * std::shared_ptr<localization::LocalizationEstimate> localization_estimate;
   * std::shared_ptr<perception::TrafficLightDetection>traffic_light;
   * std::shared_ptr<routing::RoutingResponse> routing;
   * std::shared_ptr<relative_map::MapMsg> relative_map;
   * std::shared_ptr<PadMessage> pad_msg;
   * std::shared_ptr<storytelling::Stories> stories;
   * };
   *
   * LocalView会保存着Planning所需要的信息
   * **/
  // 步骤2
  local_view_.prediction_obstacles = prediction_obstacles;
  local_view_.chassis = chassis;
  local_view_.localization_estimate = localization_estimate;
  {
    // 使用 local_view_.routing 和 routing_ 对象进行比较，判断当前的路由信息是否与本地视图中的路由信息相同。
    // 如果本地视图中的路由信息为空，或者当前的路由信息与本地视图中的路由信息不同，则需要更新本地视图中的路由信息。
    std::lock_guard<std::mutex> lock(mutex_);
    if (!local_view_.routing ||
        hdmap::PncMap::IsNewRouting(*local_view_.routing, routing_)) {
          local_view_.routing = std::make_shared<routing::RoutingResponse>(routing_);
        }
  }
  {
    // 用于更新本地视图中的交通灯检测信息和相对地图信息
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.traffic_light = std::make_shared<TrafficLightDetection>(traffic_light_);
    local_view_.relative_map = std::make_shared<MapMsg>(relative_map_);
  }
  {
    // 用于更新本地视图中的 PAD 消息信息。具体来说，该方法会将当前的 PAD 消息信息存储到本地视图中。
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.pad_msg = std::make_shared<PadMessage>(pad_msg_);
  }
  {
    // 用于更新本地视图中的故事信息。具体来说，该方法会将当前的故事信息存储到本地视图中。
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.stories = std::make_shared<Stories>(stories_);
  }

  // 用于检查输入数据是否准备就绪。具体来说，
  // 该方法会检查本地视图中的定位、底盘、地图、相对地图或路由信息是否准备就绪，
  // 如果有任何一个信息不准备就绪，则返回 false，表示输入数据不可用。
  if (!CheckInput()) {
    AERROR << "Input check failed";
    return false;
  }

  // learning_mode: NO_LEARNING
  // 如果配置文件中的 learning_mode 不为 NO_LEARNING，
  // 则会将本地视图中的底盘、障碍物预测、路由、stories、交通灯检测和定位信息传递给 message_process_ 对象进行处理。
  if (config_.learning_mode() != PlanningConfig::NO_LEARNING) {
    // data process for online training
    message_process_.OnChassis(*local_view_.chassis);
    message_process_.OnPrediction(*local_view_.prediction_obstacles);
    message_process_.OnRoutingResponse(*local_view_.routing);
    message_process_.OnStoryTelling(*local_view_.stories);
    message_process_.OnTrafficLightDetection(*local_view_.traffic_light);
    message_process_.OnLocalization(*local_view_.localization_estimate);
  }

  // publish learning data frame for RL test
  // 如果配置文件中的 learning_mode 为 RL_TEST，则会从学习数据管理器中获取最新的学习数据帧，
  // 并将其转换为 PlanningLearningData 消息类型，然后将该消息类型发送到消息总线中。
  if (config_.learning_mode() == PlanningConfig::RL_TEST) {
    PlanningLearningData planning_learning_data;
    LearningDataFrame* learning_data_frame =
        injector_->learning_based_data()->GetLatestLearningDataFrame();
    if (learning_data_frame) {
      planning_learning_data.mutable_learning_data_frame()->CopyFrom(
          *learning_data_frame);
      common::util::FillHeader(node_->Name(), &planning_learning_data);
      planning_learning_data_writer_->Write(planning_learning_data);
    } else {
      AERROR << "fail to generate learning data frame";
      return false;
    }
    return true;
  }

  /**
   * ADCTrajectory在下面文件中定义：
   * modules/planning/proto/planning.proto
   * 
   * RunOnce是规划模块的核心代码
   * 根据local_view_保存的信息，生成一条adc_trajectory_pb
   * **/
  // 步骤3 
  // 创建一个 ADCTrajectory 对象，并使用 planning_base_->RunOnce() 方法运行规划器，生成自动驾驶车辆的轨迹信息
  ADCTrajectory adc_trajectory_pb;
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  common::util::FillHeader(node_->Name(), &adc_trajectory_pb);

  // modify trajectory relative time due to the timestamp change in header
  // 获取轨迹信息中的起始时间戳，并将其赋值给 start_time 变量
  auto start_time = adc_trajectory_pb.header().timestamp_sec();
  // 计算起始时间戳与当前时间戳之间的时间差，并将其赋值给 dt 变量。
  const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
  // 获取轨迹信息中的所有轨迹点，并遍历每个轨迹点。
  for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
    // 对于每个轨迹点，使用 p.relative_time() 方法获取其相对时间戳，并将其加上 dt，以调整轨迹点的时间戳。
    p.set_relative_time(p.relative_time() + dt);
  }

  // 步骤4
  // 将轨迹信息发送到消息总线中
  planning_writer_->Write(adc_trajectory_pb);

  // record in history

  // 步骤5
  // 并使用 injector_->history()->Add() 方法将轨迹信息记录到历史轨迹中。
  auto* history = injector_->history();
  history->Add(adc_trajectory_pb);

  return true;
}

/**
 * rerouting对应的proto
 * modules/planning/proto/planning_status.proto
 * message ReroutingStatus {
 *  optional double last_rerouting_time = 1;
 *  optional bool need_rerouting = 2 [default = false];
 *  optional apollo.routing.RoutingRequest routing_request = 3;
 * }
 *  **/
void PlanningComponent::CheckRerouting() {
  auto* rerouting = injector_->planning_context()
                        ->mutable_planning_status()
                        ->mutable_rerouting();
  // modules/planning/proto/planning_status.proto
  if (!rerouting->need_rerouting()) {
    return;
  }
  // 重新规划请求设置头部信息，以便其他模块可以识别该请求的来源
  common::util::FillHeader(node_->Name(), rerouting->mutable_routing_request());
  rerouting->set_need_rerouting(false);
  // 使用 rerouting_writer_ 将重新规划请求发送到消息总线中。
  rerouting_writer_->Write(rerouting->routing_request());
}

bool PlanningComponent::CheckInput() {
  /**
   * ADCTrajectory在下面文件中定义：
   * modules/planning/proto/planning.proto
   * **/
  ADCTrajectory trajectory_pb;
  // 用 trajectory_pb.mutable_decision() 方法获取 ADCTrajectory 对象中的 decision 字段
  // 用 mutable_main_decision() 方法获取其中的 main_decision 字段
  // 使用 mutable_not_ready() 方法获取 main_decision 字段中的 not_ready 字段，并将其赋值给 not_ready 变量。
  // ADCTrajectory 是一个 protobuf 消息类型，用于存储自动驾驶车辆的轨迹信息。
  // 在该代码中，使用 ADCTrajectory 对象的 decision 字段来存储决策信息，
  // 其中的 main_decision 字段用于存储主要决策，例如是否停车、是否变道等。not_ready 字段用于表示主要决策是否准备就绪。
  auto* not_ready = trajectory_pb.mutable_decision()->mutable_main_decision()->mutable_not_ready();

  // 检查本地视图中的定位、底盘和地图信息是否准备就绪，
  // 如果有任何一个信息不准备就绪，则设置 not_ready 字段的原因为相应的信息未准备就绪。
  if (local_view_.localization_estimate == nullptr) {
    not_ready->set_reason("localization not ready");
  } else if (local_view_.chassis == nullptr) {
    not_ready->set_reason("chassis not ready");
  } else if (HDMapUtil::BaseMapPtr() == nullptr) {
    not_ready->set_reason("map not ready");
  } else {
    // nothing
  }

  /**
   * modules/common/configs/config_gflags.cc
   *
   * DEFINE_bool(use_navigation_mode, false,
   *         "Use relative position in navigation mode");
   * **/
  // 检查本地视图中的相对地图或路由信息是否准备就绪，如果未准备就绪，则设置 not_ready 字段的原因为相应的信息未准备就绪。
  if (FLAGS_use_navigation_mode) {
    if (!local_view_.relative_map->has_header()) {
      not_ready->set_reason("relative map not ready");
    }
  } else {
    if (!local_view_.routing->has_header()) {
      not_ready->set_reason("routing not ready");
    }
  }

  // 会检查 not_ready 字段是否包含reason，如果包含reason，则表示主要决策不准备就绪，
  // 将原因记录到日志中，并使用 common::util::FillHeader() 方法为轨迹消息设置头部信息，然后将轨迹消息发送到消息总线中，并返回 false。
  if (not_ready->has_reason()) {
    AERROR << not_ready->reason() << "; skip the planning cycle.";
    common::util::FillHeader(node_->Name(), &trajectory_pb);
    planning_writer_->Write(trajectory_pb);
    return false;
  }
  return true;
}

modules/planning/on_lane_planning.cc

RunOnce

// >RunOnce() 方法运行规划器,生成自动驾驶车辆的轨迹信息，并将其存储到 ptr_trajectory_pb 指针所指向的对象中
void OnLanePlanning::RunOnce(const LocalView& local_view, ADCTrajectory* const ptr_trajectory_pb) {
  // when rerouting, reference line might not be updated. In this case, planning
  // module maintains not-ready until be restarted.
  static bool failed_to_update_reference_line = false;
  // 更新本地视图
  local_view_ = local_view;
  // 记录当前时间戳和系统时间戳
  const double start_timestamp = Clock::NowInSeconds();
  // 获取当前系统时间戳，并将其转换为秒数
  const double start_system_timestamp = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();

  // 用于在调试模式下输出本地视图中的定位和底盘信息,使用 ADEBUG 宏输出本地视图中的底盘信息的调试字符串，调试字符串中包含了底盘信息的各个字段的值。
  ADEBUG << "Get localization:" << local_view_.localization_estimate->DebugString();
  ADEBUG << "Get chassis:" << local_view_.chassis->DebugString();

  // 步骤1：使用 injector_->vehicle_state() 方法获取车辆状态管理器，
  // 并使用 local_view_.localization_estimate 和 local_view_.chassis 变量作为参数，
  // 调用 Update() 方法进行车辆状态更新。更新后的车辆状态信息存储在车辆状态管理器中。
  Status status = injector_->vehicle_state()->Update(*local_view_.localization_estimate, *local_view_.chassis);

  /**
   * modules/common/vehicle_state/proto/vehicle_state.proto
   * message VehicleState {
   *   optional double x = 1 [default = 0.0];
   *   optional double y = 2 [default = 0.0];
   *   optional double z = 3 [default = 0.0];
   *   optional double timestamp = 4 [default = 0.0];
   *   optional double roll = 5 [default = 0.0];
   *   optional double pitch = 6 [default = 0.0];
   *   optional double yaw = 7 [default = 0.0];
   *   optional double heading = 8 [default = 0.0];
   *   optional double kappa = 9 [default = 0.0];
   *   optional double linear_velocity = 10 [default = 0.0];
   *   optional double angular_velocity = 11 [default = 0.0];
   *   optional double linear_acceleration = 12 [default = 0.0];
   *   optional apollo.canbus.Chassis.GearPosition gear = 13;
   *   optional apollo.canbus.Chassis.DrivingMode driving_mode = 14;
   *   optional apollo.localization.Pose pose = 15;
   *   optional double steering_percentage = 16;
   * }
   * **/
  // 获取车辆状态信息
  VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state();
  // 获取车辆状态信息的时间戳
  const double vehicle_state_timestamp = vehicle_state.timestamp();
  // 使用 DCHECK_GE 宏检查当前时间戳是否晚于车辆状态信息的时间戳。
  // 如果当前时间戳早于车辆状态信息的时间戳，则会触发断言，并输出错误信息，指出当前时间戳比车辆状态信息的时间戳早了多少秒。
  DCHECK_GE(start_timestamp, vehicle_state_timestamp)
      << "start_timestamp is behind vehicle_state_timestamp by " << start_timestamp - vehicle_state_timestamp << " secs";

  // 步骤2： 使用 status.ok() 和 util::IsVehicleStateValid() 方法判断车辆状态是否有效
  if (!status.ok() || !util::IsVehicleStateValid(vehicle_state)) {
    // 如果车辆状态无效，则生成停车轨迹，并将停车原因设置为车辆状态无效
    const std::string msg =
        "Update VehicleStateProvider failed "
        "or the vehicle state is out dated.";
    AERROR << msg;
    ptr_trajectory_pb->mutable_decision()->mutable_main_decision()->mutable_not_ready()->set_reason(msg);
    // 将错误信息保存到轨迹消息的状态中
    status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());
    // 将轨迹消息的档位设置为 DRIVE
    ptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    // 使用 FillPlanningPb() 方法填充轨迹消息的头部信息。
    FillPlanningPb(start_timestamp, ptr_trajectory_pb);
    // 会清空轨迹消息中的所有轨迹点，然后根据当前车辆状态生成一系列停车轨迹点，并将其添加到轨迹消息中。
    GenerateStopTrajectory(ptr_trajectory_pb);
    return;
  }

  /**
   * modules/planning/common/planning_gflags.cc
   *
   * DEFINE_double(message_latency_threshold, 0.02,
   *  "Threshold for message delay");
   * **/

  // 步骤3：
  // 如果当前时间与车辆状态时间戳比较接近(<0.02),则更新当前车辆状态(主要是位置)
  if (start_timestamp - vehicle_state_timestamp < FLAGS_message_latency_threshold) {
    // 调用 AlignTimeStamp() 方法将车辆状态信息的时间戳校准为当前时间戳，并将校准后的车辆状态信息赋值给 vehicle_state 变量
    vehicle_state = AlignTimeStamp(vehicle_state, start_timestamp);
  }

  // 步骤4： 判断当前收到的routing是否和上一次相同
  if (util::IsDifferentRouting(last_routing_, *local_view_.routing)) {
    // 如果不同，则将当前路由赋值给 last_routing_ 变量
    last_routing_ = *local_view_.routing;
    ADEBUG << "last_routing_:" << last_routing_.ShortDebugString();
    // 清空历史信息
    injector_->history()->Clear();
    injector_->planning_context()->mutable_planning_status()->Clear();
    // 更新参考线提供器，并使用 planner_->Init() 方法初始化规划器。
    reference_line_provider_->UpdateRoutingResponse(*local_view_.routing);
    planner_->Init(config_);
  }

  // 步骤5：用于检查参考线提供器是否更新失败,如果更新失败，则将 failed_to_update_reference_line 变量设置为 true。
  failed_to_update_reference_line = (!reference_line_provider_->UpdatedReferenceLine());

  // 如果参考线提供器更新失败，则生成停车轨迹，并将停车原因设置为参考线更新失败
  if (failed_to_update_reference_line) {
    const std::string msg = "Failed to update reference line after rerouting.";
    AERROR << msg;
    ptr_trajectory_pb->mutable_decision()->mutable_main_decision()->mutable_not_ready()->set_reason(msg);
    // 使用 status.Save() 方法将错误信息保存到轨迹消息的状态中。
    status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());
    // 将轨迹消息的档位设置为 DRIVE
    ptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    // 使用 FillPlanningPb() 方法填充轨迹消息的头部信息。
    FillPlanningPb(start_timestamp, ptr_trajectory_pb);
    // 使用 GenerateStopTrajectory() 方法生成停车轨迹，并返回
    GenerateStopTrajectory(ptr_trajectory_pb);
    return;
  }

  // 步骤6：将当前车辆状态信息更新到参考线提供器中，以便参考线提供器能够根据当前车辆状态信息提供相应的参考线。
  reference_line_provider_->UpdateVehicleState(vehicle_state);

  // planning is triggered by prediction data, but we can still use an estimated
  // cycle time for stitching
  /**
   * DEFINE_int32(planning_loop_rate, 10, "Loop rate for planning node");
   *
   * **/
  // 步骤7：计planning周期,默认0.1秒
  const double planning_cycle_time = 1.0 / static_cast<double>(FLAGS_planning_loop_rate);

  std::string replan_reason;

  /**
   *
   * DEFINE_uint64(trajectory_stitching_preserved_length, 20,
   *           "preserved points number in trajectory stitching");
   *
   * **/
  // 步骤8：计算拼接轨迹，并将计算结果存储在 stitching_trajectory 变量中。
  // FLAGS_trajectory_stitching_preserved_length：接轨迹保留的长度
  std::vector<TrajectoryPoint> stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(vehicle_state, start_timestamp, 
        planning_cycle_time,FLAGS_trajectory_stitching_preserved_length, true,
        last_publishable_trajectory_.get(), &replan_reason);

  /**
   * 计算车辆的包围框
   * **/
  // 步骤9：根据拼接后轨迹中的最后一个点，更新车辆的位置、速度、加速度、朝向等信息。
  // 同时，在计算车辆的包围盒信息时，需要根据车辆的位置、朝向和车辆的长度、宽度等信息，计算车辆的包围盒信息。
  injector_->ego_info()->Update(stitching_trajectory.back(), vehicle_state);

  // 首先根据当前时间戳，生成一个帧编号 frame_num
  const uint32_t frame_num = static_cast<uint32_t>(seq_num_++);

  // 步骤10： 核心代码，初始化一个新的规划帧
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);

  // 检查规划帧的初始化状态，如果初始化成功，
  // 则调用车辆信息的 CalculateFrontObstacleClearDistance 方法，计算车辆前方障碍物的安全距离
  if (status.ok()) {
    injector_->ego_info()->CalculateFrontObstacleClearDistance(frame_->obstacles());
  }

  /**
   * DEFINE_bool(enable_record_debug, true,
   *         "True to enable record debug info in chart format");
   * **/
  // 首先检查是否启用调试记录功能。如果启用，则调用规划帧的 RecordInputDebug 方法，将调试信息记录到轨迹信息中
  if (FLAGS_enable_record_debug) {
    frame_->RecordInputDebug(ptr_trajectory_pb->mutable_debug());
  }

  // 将当前时间与之前记录的时间戳之间的差异（以毫秒为单位）设置为latency_stats消息中的init_frame_time_ms字段的值
  ptr_trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms(
      Clock::NowInSeconds() - start_timestamp);

  if (!status.ok()) {
    AERROR << status.ToString();
    if (FLAGS_publish_estop) {
      // "estop" signal check in function "Control::ProduceControlCommand()"
      // estop_ = estop_ || local_view_.trajectory.estop().is_estop();
      // we should add more information to ensure the estop being triggered.
      ADCTrajectory estop_trajectory;
      // 获取 ADCTrajectory 对象的 estop 字段
      EStop* estop = estop_trajectory.mutable_estop();
      // 表示发生了紧急停车
      estop->set_is_estop(true);
      // 将错误的详细信息设置为 estop 字段的 reason 属性，以提供关于紧急停车的原因的更多信息
      estop->set_reason(status.error_message());
      //  将 status 对象的一些信息保存到 estop_trajectory 的头部
      status.Save(estop_trajectory.mutable_header()->mutable_status());
      ptr_trajectory_pb->CopyFrom(estop_trajectory);
    } else {
      ptr_trajectory_pb->mutable_decision()
          ->mutable_main_decision()
          ->mutable_not_ready()
          ->set_reason(status.ToString());
      status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());
      // 调用GenerateStopTrajectory()函数生成停止轨迹
      GenerateStopTrajectory(ptr_trajectory_pb);
    }
    // 将车辆的档位设置为驾驶模式，然后调用FillPlanningPb()函数填充规划ptr_trajectory_pb
    ptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    FillPlanningPb(start_timestamp, ptr_trajectory_pb);
    frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);
    // 获取当前帧的序列号n，并将帧添加到帧历史记录中
    const uint32_t n = frame_->SequenceNum();
    injector_->frame_history()->Add(n, std::move(frame_));
    return;
  }

  // 步骤11. 它遍历参考线信息，并对每个参考线信息执行交通决策。
  for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {
    TrafficDecider traffic_decider;
    traffic_decider.Init(traffic_rule_configs_);
    // ，调用TrafficDecider对象的Execute()方法执行交通决策。
    auto traffic_status = traffic_decider.Execute(frame_.get(), &ref_line_info, injector_);
    // 如果交通决策执行成功，则继续执行下一个参考线信息
    // 如果交通决策执行失败或参考线信息不可行，则将参考线信息标记为不可行，并输出一条警告日志。
    if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {
      ref_line_info.SetDrivable(false);
      AWARN << "Reference line " << ref_line_info.Lanes().Id()
            << " traffic decider failed";
    }
  }

  // 步骤12. 调用Plan()函数来规划车辆的行驶轨迹。Plan()函数接受三个参数：起始时间戳、拼接轨迹和指向轨迹protobuf的指针。
  status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);

  for (const auto& p : ptr_trajectory_pb->trajectory_point()) {
    // 使用ADEBUG宏输出每个轨迹点的调试信息。
    ADEBUG << p.DebugString();
  }
  
  // 计算代码执行时间:获取当前系统时间，并计算从start_system_timestamp到当前时间的时间差
  const auto end_system_timestamp = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
  const auto time_diff_ms = (end_system_timestamp - start_system_timestamp) * 1000;
  ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";
  // 将计算得到的时间差保存到轨迹protobuf的延迟统计信息中
  ptr_trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << ptr_trajectory_pb->latency_stats().DebugString();

  // 处理规划轨迹时出现的错误情况，并在必要时设置紧急停车信号，以确保车辆安全行驶。
  if (!status.ok()) {
    status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());
    AERROR << "Planning failed:" << status.ToString();
    if (FLAGS_publish_estop) { // 设置紧急停车信号
      AERROR << "Planning failed and set estop";
      // "estop" signal check in function "Control::ProduceControlCommand()"
      // estop_ = estop_ || local_view_.trajectory.estop().is_estop();
      // we should add more information to ensure the estop being triggered.
      EStop* estop = ptr_trajectory_pb->mutable_estop();
      estop->set_is_estop(true);
      estop->set_reason(status.error_message());
    }
  }
  // 如果重新规划标志为true，则使用set_replan_reason()方法设置重新规划的原因
  ptr_trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);
  if (ptr_trajectory_pb->is_replan()) {
    ptr_trajectory_pb->set_replan_reason(replan_reason);
  }

  // 首先检查车辆是否在开放空间轨迹上行驶，
  // 如果是，则调用FillPlanningPb()函数填充规划protobuf，并将规划protobuf设置为当前帧的规划轨迹
  if (frame_->open_space_info().is_on_open_space_trajectory()) {
    FillPlanningPb(start_timestamp, ptr_trajectory_pb);
    ADEBUG << "Planning pb:" << ptr_trajectory_pb->header().DebugString();
    frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);
  } else { // 用于处理车辆在普通道路上行驶的情况
    // 首先添加参考线提供者的延迟统计信息到轨迹protobuf的延迟统计信息中，并设置车辆的驾驶模式为前进模式
    auto* ref_line_task = ptr_trajectory_pb->mutable_latency_stats()->add_task_stats();
    ref_line_task->set_time_ms(reference_line_provider_->LastTimeDelay() * 1000.0);
    ref_line_task->set_name("ReferenceLineProvider");
    // TODO(all): integrate reverse gear
    ptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    FillPlanningPb(start_timestamp, ptr_trajectory_pb);
    ADEBUG << "Planning pb:" << ptr_trajectory_pb->header().DebugString();
    // 将规划protobuf设置为当前帧的规划轨迹。
    frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);
    if (FLAGS_enable_planning_smoother) {
      // 调用planning_smoother_.Smooth()方法对规划轨迹进行平滑处理。
      planning_smoother_.Smooth(injector_->frame_history(), frame_.get(), ptr_trajectory_pb);
    }
  }
  // 先获取当前帧的序列号n，然后调用Add()方法将当前帧添加到帧历史记录中。
  const uint32_t n = frame_->SequenceNum();
  injector_->frame_history()->Add(n, std::move(frame_));
}

ComputeStitchingTrajectory
于根据当前车辆状态信息和上一条轨迹，计算出拼接后的轨迹，并返回拼接后的轨迹。

std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time, const size_t preserved_points_num,
    const bool replan_by_offset, const PublishableTrajectory* prev_trajectory,
    std::string* replan_reason) {
  /**
   * DEFINE_bool(enable_trajectory_stitcher, true,
   *    "enable stitching trajectory");
   * **/
  // 检查拼接轨迹是否启用
  if (!FLAGS_enable_trajectory_stitcher) {
    *replan_reason = "stitch is disabled by gflag.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

  // 首先判断上一条轨迹是否存在。如果上一条轨迹不存在，则说明需要重新规划轨迹。
  if (!prev_trajectory) {
    *replan_reason = "replan for no previous trajectory.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

  // 判断车辆的驾驶模式是否为完全自动驾驶模式。如果车辆的驾驶模式不是完全自动驾驶模式，则说明车辆处于手动驾驶模式。
  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    *replan_reason = "replan for manual mode.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

  // 于判断上一条轨迹是否为空。如果上一条轨迹为空，则需要重新规划轨迹。
  size_t prev_trajectory_size = prev_trajectory->NumOfPoints();
  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    *replan_reason = "replan for empty previous trajectory.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }

  // 首先计算出当前车辆在上一条轨迹上的相对时间 veh_rel_time
  const double veh_rel_time = current_timestamp - prev_trajectory->header_time();
  // 在上一条轨迹中查找当前车辆所在的轨迹点,会返回当前车辆所在的轨迹点的下标，
  size_t time_matched_index = prev_trajectory->QueryLowerBoundPoint(veh_rel_time);
  // 如果当前车辆所在的轨迹点是上一条轨迹的第一个点，并且当前时间小于上一条轨迹的起始时间
  // ，则说明当前时间不在上一条轨迹的时间范围内,重新规划轨迹。
  if (time_matched_index == 0 && veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "current time smaller than the previous trajectory's first time";
    *replan_reason = "replan for current time smaller than the previous trajectory's first time.";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }
  // 如果当前车辆所在的轨迹点是上一条轨迹的最后一个点，则说明当前时间超出了上一条轨迹的时间范围。此时，将重新规划
  if (time_matched_index + 1 >= prev_trajectory_size) {
    AWARN << "current time beyond the previous trajectory's last time";
    *replan_reason =
        "replan for current time beyond the previous trajectory's last time";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }
  // 获取该轨迹点的详细信息，包括位置、速度、加速度、曲率等信息
  auto time_matched_point = prev_trajectory->TrajectoryPointAt(static_cast<uint32_t>(time_matched_index));
  // 如果该轨迹点不包含路径点信息,重新规划轨迹。
  if (!time_matched_point.has_path_point()) {
    *replan_reason = "replan for previous trajectory missed path point";
    return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
  }
  // 首先根据当前车辆状态信息，包括车辆的横向和纵向位置，调用 QueryNearestPointWithBuffer 方法，
  // 在上一条轨迹中查找距离当前车辆最近的轨迹点。该方法会返回当前车辆所在的轨迹点的下标
  size_t position_matched_index = prev_trajectory->QueryNearestPointWithBuffer(
      {vehicle_state.x(), vehicle_state.y()}, 1.0e-6);
  // 计算出车辆在轨迹上的横向和纵向偏差。该方法会返回一个 FrenetFramePoint 类型的结构体，
  // 包含了车辆在轨迹上的横向和纵向偏差，以及车辆在轨迹上的曲率和速度。
  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(),
      prev_trajectory->TrajectoryPointAt(static_cast<uint32_t>(position_matched_index)));
  // 首先判断是否需要根据偏移量重新规划轨迹
  if (replan_by_offset) {
    // 则计算当前车辆在轨迹上的横向和纵向偏差
    auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first;
    auto lat_diff = frenet_sd.second;

    ADEBUG << "Control lateral diff: " << lat_diff << ", longitudinal diff: " << lon_diff;
    // 判断横向偏差是否超过阈值。如果横向偏差超过阈值，则说明当前车辆已经偏离了原来的轨迹，需要重新规划轨迹。
    if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold) { // 0.5
      const std::string msg = absl::StrCat(
          "the distance between matched point and actual position is too "
          "large. Replan is triggered. lat_diff = ",lat_diff);
      AERROR << msg;
      *replan_reason = msg;
      return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                              vehicle_state);
    }
    // 判断纵向偏差是否超过阈值。如果纵向偏差超过阈值，则说明当前车辆已经偏离了原来的轨迹，需要重新规划轨迹。
    if (std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) { // 2.5
      const std::string msg = absl::StrCat(
          "the distance between matched point and actual position is too "
          "large. Replan is triggered. lon_diff = ",lon_diff);
      AERROR << msg;
      *replan_reason = msg;
      return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                              vehicle_state);
    }
  } else {
    ADEBUG << "replan according to certain amount of lat and lon offset is disabled";
  }
  // 首先计算出当前车辆在上一条轨迹上的相对时间 veh_rel_time，即当前时间戳减去上一条轨迹的起始时间戳
  // 根据规划周期时间，计算出下一个时刻的相对时间 forward_rel_time，即当前时间戳加上规划周期时间
  double forward_rel_time = veh_rel_time + planning_cycle_time;
  // 在上一条轨迹中查找下一个时刻的轨迹点。该方法会返回下一个时刻的轨迹点的下标，即下一个时刻的轨迹点是上一条轨迹中的第几个点。
  size_t forward_time_index = prev_trajectory->QueryLowerBoundPoint(forward_rel_time);

  ADEBUG << "Position matched index:\t" << position_matched_index;
  ADEBUG << "Time matched index:\t" << time_matched_index;
  // 首先根据当前车辆在时间轴和空间轴上匹配到的轨迹点的下标，计算出两者的最小值,作为拼接后轨迹的起始点。
  auto matched_index = std::min(time_matched_index, position_matched_index);
  // 根据保留轨迹点数和起始点的下标，截取上一条轨迹中需要保留的轨迹点，
  // 并将其存储在一个 std::vector 类型的变量 stitching_trajectory 中。
  // 最后，根据下一个时刻的轨迹点的下标，将下一个时刻的轨迹点添加到 stitching_trajectory 中。
  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->begin() + std::max(0, static_cast<int>(matched_index - preserved_points_num)),
      prev_trajectory->begin() + forward_time_index + 1);
  ADEBUG << "stitching_trajectory size: " << stitching_trajectory.size();
  // 首先计算出拼接后轨迹的起始位置 zero_s，即拼接后轨迹中第一个点在上一条轨迹中的位置。
  const double zero_s = stitching_trajectory.back().path_point().s();
  for (auto& tp : stitching_trajectory) {
    // 遍历拼接后的轨迹中的每个点，判断该点是否包含路径点信息。如果该点不包含路径点信息，则说明上一条轨迹缺失路径点。此时，将重新规划
    if (!tp.has_path_point()) {
      *replan_reason = "replan for previous trajectory missed path point";
      return ComputeReinitStitchingTrajectory(planning_cycle_time,vehicle_state);
    }
    // 对于每个点，将其相对时间调整为相对于当前时间戳的时间，并将其在路径上的位置调整为相对于拼接后轨迹的起始位置的距离。
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() - current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  return stitching_trajectory;
}