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_));
}

src/modules/common/vehicle_state/vehicle_state_provider.cc
Update
更新后的车辆状态信息存储在车辆状态管理器vehicle_state_中。

Status VehicleStateProvider::Update(
        const localization::LocalizationEstimate &localization,
        const canbus::Chassis &chassis)
{
    AINFO << "update internal position";

    original_localization_ = localization;
    if (!ConstructExceptLinearVelocity(localization))
    {
        std::string msg = absl::StrCat(
                "Fail to update because ConstructExceptLinearVelocity error.",
                "localization:\n", localization.DebugString());

        return Status(ErrorCode::LOCALIZATION_ERROR, msg);
    }

    AINFO << "update localization speed";

    if (localization.has_measurement_time())
    {
        vehicle_state_.set_timestamp(localization.measurement_time());
    }
    else if (localization.header().has_timestamp_sec())
    {
        vehicle_state_.set_timestamp(localization.header().timestamp_sec());
    }
    else if (chassis.has_header() && chassis.header().has_timestamp_sec())
    {
        AERROR << "Unable to use location timestamp for vehicle state. Use "
                  "chassis time instead.";
        vehicle_state_.set_timestamp(chassis.header().timestamp_sec());
    }

    if (chassis.has_gear_location())
    {
        vehicle_state_.set_gear(chassis.gear_location());
    }
    else
    {
        vehicle_state_.set_gear(canbus::Chassis::GEAR_NONE);
    }

    // apollo: 速度来自imu, acc来自imu
    if (chassis.has_speed_mps())
    {
        vehicle_state_.set_linear_velocity(chassis.speed_mps());
        if (!FLAGS_reverse_heading_vehicle_state &&
            vehicle_state_.gear() == canbus::Chassis::GEAR_REVERSE)
        {
            vehicle_state_.set_linear_velocity(
                    -vehicle_state_.linear_velocity());
        }
    }

    if (chassis.has_steering_percentage())
    {
        vehicle_state_.set_steering_percentage(chassis.steering_percentage());
    }
    else
    {
        vehicle_state_.set_steering_percentage(0.0);
    }

    if (0)
    {
        // 曲率应该和方向盘角度有关系，不应该使用定位数据来计算？
        static constexpr double kEpsilon = 1e-6;
        if (std::abs(vehicle_state_.linear_velocity()) < kEpsilon)
        {
            vehicle_state_.set_kappa(0.0);
        }
        else
        {
            vehicle_state_.set_kappa(vehicle_state_.angular_velocity() /
                                     vehicle_state_.linear_velocity());
        }
    }
    else
    {
        // 从方向盘当前状态开始起步，还是0转角开始起步?

        const VehicleConfig &vehicle_config_ =
                apollo::common::VehicleConfigHelper::GetConfig();

        double front_wheel_angle =
                chassis.steering_percentage() / 100.0 *
                vehicle_config_.vehicle_param().max_steer_angle() /
                vehicle_config_.vehicle_param().steer_ratio();

        double kappa = std::tan(front_wheel_angle) /
                       vehicle_config_.vehicle_param().wheel_base();

        vehicle_state_.set_kappa(kappa);
    }

    vehicle_state_.set_driving_mode(chassis.driving_mode());

    return Status::OK();
}

bool VehicleStateProvider::ConstructExceptLinearVelocity(
        const localization::LocalizationEstimate &localization)
{
    if (!localization.has_pose())
    {
        AERROR << "Invalid localization input.";
        return false;
    }

    // skip localization update when it is in use_navigation_mode.
    if (FLAGS_use_navigation_mode)
    {
        ADEBUG << "Skip localization update when it is in use_navigation_mode.";
        return true;
    }

    vehicle_state_.mutable_pose()->CopyFrom(localization.pose());
    if (localization.pose().has_position())
    {
        // 地图参考坐标系：东北天ENU的位置
        vehicle_state_.set_x(localization.pose().position().x());
        vehicle_state_.set_y(localization.pose().position().y());
        vehicle_state_.set_z(localization.pose().position().z());
    }
    // 从IMU坐标系到世界坐标系的旋转
    const auto &orientation = localization.pose().orientation();
    // 世界坐标系的朝向
    if (localization.pose().has_heading())
    {
        vehicle_state_.set_heading(localization.pose().heading());
    }
    else
    {
        vehicle_state_.set_heading(
                math::QuaternionToHeading(orientation.qw(), orientation.qx(),
                                          orientation.qy(), orientation.qz()));
    }

    if (FLAGS_enable_map_reference_unify) // enable IMU data convert to map reference ：false
    {
        if (!localization.pose().has_angular_velocity_vrf()) // 车辆坐标系的角加速度
        {
            AERROR << "localization.pose().has_angular_velocity_vrf() must be "
                      "true "
                      "when FLAGS_enable_map_reference_unify is true.";
            return false;
        }
        vehicle_state_.set_angular_velocity(
                localization.pose().angular_velocity_vrf().z());

        if (!localization.pose().has_linear_acceleration_vrf())
        {
            AERROR << "localization.pose().has_linear_acceleration_vrf() must "
                      "be "
                      "true when FLAGS_enable_map_reference_unify is true.";
            return false;
        }
        vehicle_state_.set_linear_acceleration(
                localization.pose().linear_acceleration_vrf().y());
    }
    else
    {
        // 加速度来自定位，chassis包含硬件数据，不计算acc
        if (!localization.pose().has_angular_velocity()) // 地图参考系中车辆的角速度
        {
            AERROR << "localization.pose() has no angular velocity.";
            return false;
        }
        vehicle_state_.set_angular_velocity(
                localization.pose().angular_velocity().z());

        if (!localization.pose().has_linear_acceleration()) // 地图参考系中车辆的线性加速度
        {
            AERROR << "localization.pose() has no linear acceleration.";
            return false;
        }
        vehicle_state_.set_linear_acceleration(
                localization.pose().linear_acceleration().y());
    }

    if (localization.pose().has_euler_angles()) // 世界坐标系的欧拉角
    {
        vehicle_state_.set_roll(localization.pose().euler_angles().y());
        vehicle_state_.set_pitch(localization.pose().euler_angles().x());
        vehicle_state_.set_yaw(localization.pose().euler_angles().z());
    }
    else
    {   // 有四元素转换欧拉角
        math::EulerAnglesZXYd euler_angle(orientation.qw(), orientation.qx(),
                                          orientation.qy(), orientation.qz());
        vehicle_state_.set_roll(euler_angle.roll());
        vehicle_state_.set_pitch(euler_angle.pitch());
        vehicle_state_.set_yaw(euler_angle.yaw());
    }

    return true;
}

ComputeStitchingTrajectory

1、什么是轨迹拼接？
在每一帧轨迹规划的开始，我们需要确定规划的起始点p_0=(t,x,y,v,a,heading,kappa)，起始点p0 给定了规划的初始状态，然后结合当前帧的环境信息并调用规划算法规划生成当前帧轨迹。那么问题来了，每一帧规划的起始点p0是如何确定的呢？在Apollo规划算法中采用了两种方案：

• 方案1：
  根据ADC（Autonomous Driving Car）实际位置状态信息或根据ADC实际位置状态信息，通过车辆运动学推导0.1s后的位置状态信息，作为规划起始点状态，在Apollo中，这一方案叫做重规划（Replan），但是它属于轨迹拼接的一种，只是拼接点集只有一个点p0而已。
• 方案2：
  根据当前帧时间戳以及ADC实际位置信息，在上一帧轨迹中寻找匹配点，将上一帧轨迹中匹配点往后0.1s所对应的轨迹点作为当前帧的规划起始状态点，待当前帧规划生成轨迹后，再和上一帧轨迹中所选择的规划起始点前一段距离的轨迹点进行拼接，形成一条完整的车辆运动轨迹，发送给下游控制模块。

严格来说，以上两种方案都属于轨迹拼接，区别是方案1拼接轨迹只有一个点，而方案2的拼接轨迹是上一帧的部分有序点集，但是在Apollo代码中，称第一种方案叫做重新初始化拼接点（也叫重规划）。

2、为什么要进行轨迹拼接？
在了解了什么是轨迹拼接，不知道大家有没有思考过，轨迹拼接的方法为什么有两种，是基于什么原因想法设计的呢？
在轨迹拼接中纳入第二种方法主要考虑的是连续帧之间轨迹的平滑性和连续性，因为轨迹的这两个特性会严重影响到下游控制的效果。理论上，如果控制跟踪和定位完美，不会出现任何误差，则完全可以使用方案1选择规划起始点，但是，由于定位误差和控制误差/滞后的原因，会导致每一规划时刻ADC位置大概率的偏离上一帧所规划的轨迹点，如果此时仍采用方案1，会导致当前帧所规划轨迹和上一帧规划轨迹的不平滑和不连续，从而导致控制的抖动，也可能会引起ADC行驶轨迹发散，跟踪误差越来越大，偏离设定参考线。而方案2可以保证ADC附近一段范围内前后两帧轨迹的平滑性，从而不断引导ADC沿着期望轨迹行驶。

1.方案1：重新规划：

• 缝合轨迹被gflag禁用。

• 没有先前的轨迹。

• 不在自动驾驶模式下。

• 前一帧的点数为零。

• 当前时间小于前一帧的轨迹开始时间。

• 当前时间大于前一帧的轨迹结束时间。

• 匹配路径点为空。

• 投影点的水平和垂直偏差大于阈值。

• 根据规划周期和投影点位置缝合轨迹。

• 判断拼接轨迹的每个轨迹点是否为空，如果为空则重新规划。

方案1生成轨迹拼接点主要有两种情况：

• 起步阶段：由于起步阶段属于规划算法执行的第一帧，且速度、加速度值很小，可以直接使用当前状态点作为规划的起始状态点
• 非起步阶段：则根据当前ADC状态点结合运动学模型外推0.1s时间之后的状态点作为本帧规划起始点

2.方案2：同时考虑了时间匹配和位置匹配，轨迹拼接的具体方法为：

• 对于时间匹配：
  根据当前时间戳和上一帧轨迹起点的时间戳对比，计算当前时间ADC应该到达的时间匹配点（图1中t_pos）及该匹配点在上一帧轨迹中对应的索引值t_index，若t_pos不存在路径点，则选择方案1。

• 对于位置匹配：
  根据ADC当前位置点在上一帧轨迹中寻找最近的匹配点（图1中p_pos）及该匹配点在上一帧轨迹中对应的索引值p_index，并比较实际位置点和匹配点之间的横纵向位置偏差，若任一偏差超过给定阈值，则选择方案1。

若以上判断均通过，根据 （当前时刻的绝对时间-上一时刻轨迹的初始时间+planning_cycle_time） 得到forward_rel_time，选取时间匹配索引和位置匹配索引中的较小索引作为匹配点索引值matched_index，在上一帧的轨迹上截取出matched_index往前n个点开始，至forward_rel_time的一段轨迹，作为stitching_trajectory。最后，遍历这段轨迹，对其relative_time和s进行赋值。最终，使用stitching_trajectory的最后一个点，作为当前帧轨迹规划的起始点。

选择当前时间+planning_cycle_time的时间对应的上一帧轨迹的位置点作为本帧规划起点的原因是，当本帧规划需要时间planning_cycle_time，将生成轨迹送到规划模块时对应的实际时间理论上就是当前时间+planning_cycle_time。

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)
{
    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);
    }

    // 相对于上一帧轨迹的时间，轨迹时间是planning模块的起始时间
    const double veh_rel_time = current_timestamp - prev_trajectory->header_time();

    // AINFO << std::fixed << std::setprecision(3) << current_timestamp;
    // AINFO << std::fixed << std::setprecision(3)
    //       << prev_trajectory->header_time();
    // AINFO << std::fixed << std::setprecision(3) << veh_rel_time;
    // 在上一条轨迹中查找当前车辆所在的轨迹点,会返回当前车辆所在的轨迹点的下标
    size_t time_matched_index = prev_trajectory->QueryLowerBoundPoint(veh_rel_time);

    // 历史轨迹起点是当前时刻0.1秒后的预测点，下一次规划开始时，
    // 投影点可能就是上一个轨迹的起点，因为上一次预测很准，这样就导致不停重规划，这是不期望的，
    // 总是从车辆状态开始规划，会引起方向盘振荡。
    // 所以，关闭这个条件？
    double time_error = 0.1;
    if (time_matched_index == 0 && veh_rel_time + time_error < prev_trajectory->StartPoint().relative_time())
    {
        AWARN << "current time smaller than the previous trajectory's first time";

        AINFO << veh_rel_time << " , " << prev_trajectory->StartPoint().relative_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);

    // 相对于轨迹的sl.
    // position_matched_index
    // 这个值在停车状态下，可能很大。因为t在增长，s不变，如果根据位置点得到轨迹索引，
    // 那么生成的轨迹会越来越长
    // 计算出车辆在轨迹上的横向和纵向偏差。该方法会返回一个 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)
        // if (0)
        {
            const std::string msg = absl::StrCat(
                    "the distance between matched point and actual "
                    "position is "
                    "too large. Replan is triggered. lat_diff = ",
                    lat_diff);
            AINFO << msg;
            *replan_reason = msg;

            return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                                    vehicle_state);
        }

        // 2.5米。
        if (replan_by_lon_offset)
        {
            // 判断纵向偏差是否超过阈值。如果纵向偏差超过阈值，则说明当前车辆已经偏离了原来的轨迹，需要重新规划轨迹。
            if (std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold)
            {
                const std::string msg = absl::StrCat(
                        "the distance between matched point and actual "
                        "position is "
                        "too large. Replan is triggered. lon_diff = ",
                        lon_diff);

                *replan_reason = msg;
                AINFO << msg;

#if debug_traj_stitch
                
                AINFO << "veh speed: " << vehicle_state.linear_velocity();
                AINFO << "Position matched index: " << position_matched_index;
                AINFO << "Time matched index: " << time_matched_index;

                for (size_t i = 0; i < prev_trajectory->size(); i++)
                {
                    AINFO << "path point id: " << i;
                    AINFO << prev_trajectory->at(i).DebugString();

                    if (i > position_matched_index && i > time_matched_index)
                    {
                        break;
                    }
                }

#endif
                return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
            }
        }
    }

    // 基本在200毫秒左右
    // 首先计算出当前车辆在上一条轨迹上的相对时间 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);

    // 首先根据当前车辆在时间轴和空间轴上匹配到的轨迹点的下标，计算出两者的最小值,作为拼接后轨迹的起始点。
    auto start_pt_index = std::min(time_matched_index, position_matched_index);

    int stitch_start_point;
    stitch_start_point = std::max(0, static_cast<int>(start_pt_index - preserved_points_num));

    // stitch traj end
    int stitch_end_point;

    // 如果底盘的速度很低，轨迹起点依然选择车辆后轴附近
    if (vehicle_state.linear_velocity() < 0.02)
    {
        stitch_end_point = std::min(forward_time_index, position_matched_index);
        stitch_end_point += 1;
    }
    else
    {
        stitch_end_point = forward_time_index + 1;
    }
    // 根据保留轨迹点数和起始点的下标，截取上一条轨迹中需要保留的轨迹点，
    // 并将其存储在一个 std::vector 类型的变量 stitching_trajectory 中。
    // 最后，根据下一个时刻的轨迹点的下标，将下一个时刻的轨迹点添加到 stitching_trajectory 中。
    std::vector<TrajectoryPoint> stitching_trajectory(
            prev_trajectory->begin() + stitch_start_point,
            prev_trajectory->begin() + stitch_end_point);

#if debug_traj_stitch
    AINFO << "forward_rel_time: " << forward_rel_time;
    AINFO << "forward_time_index: " << forward_time_index;

    AINFO << "veh speed: " << vehicle_state.linear_velocity();
    AINFO << "Position matched index: " << position_matched_index;
    AINFO << "Time matched index: " << time_matched_index;
    AINFO << "stitch_start_point: " << stitch_start_point;
    AINFO << "stitch_end_point: " << stitch_end_point;

    for (size_t i = 0; i < prev_trajectory->size(); i++)
    {
        AINFO << "id: " << i;
        AINFO << prev_trajectory->at(i).DebugString();
    }

    AINFO << "stitching_trajectory size: " << stitching_trajectory.size();

#endif
    // 轨迹点的时间基准是：当前帧时间戳.
    // 轨迹点的s基准是：历史轨迹上的车辆期望点
    // 最后一个点作为当前帧轨迹规划的起始点
    const double zero_s = stitching_trajectory.back().path_point().s();
    // 获取拼接后轨迹中最后一个点的纵向距离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);

        // AINFO << tp.relative_time();
        // AINFO << tp.path_point().s();
    }
    return stitching_trajectory;
}

QueryLowerBoundPoint
modules/planning/common/trajectory/discretized_trajectory.cc

size_t DiscretizedTrajectory::QueryLowerBoundPoint(const double relative_time,const double epsilon) const
{
    ACHECK(!empty());

    // 判断relative_time是否大于等于离散轨迹中最后一个点的相对时间
    if (relative_time >= back().relative_time())
    {
        // 如果是则返回离散轨迹中最后一个点的下标
        return size() - 1;
    }

    // 否则，使用std::lower_bound()函数在离散轨迹中查找相对时间最接近的点，并返回该点的下标。
    auto func = [&epsilon](const TrajectoryPoint& tp,const double relative_time) {
        return tp.relative_time() + epsilon < relative_time;
    };

    auto it_lower = std::lower_bound(begin(), end(), relative_time, func);
    
    return std::distance(begin(), it_lower); // 用于计算两个迭代器之间的距离
}

EgoInfo::Update

bool EgoInfo::Update(const common::TrajectoryPoint& start_point,
                     const common::VehicleState& vehicle_state)
{
    set_start_point(start_point);
    set_vehicle_state(vehicle_state);
    CalculateEgoBox(vehicle_state);
    return true;
}

    void set_start_point(const common::TrajectoryPoint& start_point)
    {
        start_point_ = start_point;
        // 获取车辆的参数信息，包括车辆的最大加速度和最大减速度
        const auto& param = ego_vehicle_config_.vehicle_param();
        // 在限制车辆的加速度时，需要根据车辆的最大加速度和最大减速度，限制车辆的加速度
        start_point_.set_a(std::fmax(std::fmin(start_point_.a(), param.max_acceleration()),param.max_deceleration()));
    }

    void set_vehicle_state(const common::VehicleState& vehicle_state)
    {
        vehicle_state_ = vehicle_state;
    }

/**
 * 得到车辆的包围框
 * **/
void EgoInfo::CalculateEgoBox(const common::VehicleState& vehicle_state) {
  const auto& param = ego_vehicle_config_.vehicle_param();
  ADEBUG << "param: " << param.DebugString();
  // 计算出车辆中心到车辆前后边缘中心和车辆左右边缘中心的距
  Vec2d vec_to_center(
      (param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,
      (param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);

  Vec2d position(vehicle_state.x(), vehicle_state.y());
  Vec2d center(position + vec_to_center.rotate(vehicle_state.heading()));
  // 根据车辆的中心点、朝向、长度和宽度等信息，计算出车辆的包围盒信息 ego_box_
  ego_box_ = Box2d(center, vehicle_state.heading(), param.length(), param.width());
}