目录结构介绍
cyber  消息中间件，替换ros作为消息层
data  地图等生成好的数据放在这里（其他数据待补充）
docker 容器相关
docs 文档相关
modules 定位，预测，感知，规划等自动驾驶模块
scripts 脚本
third_party 第三方库
tools 工具目录，几乎为空

Cyber模块

Cyber main函数中先根据命令行信息解析dag参数，然后利用解析的参数，动态的加载对应的模块

输入：命令行数量+命令行内容

输出：加载状态

关联模块：All

mainboard.cc

/******************************************************************************
 * Copyright 2018 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 "cyber/common/global_data.h"
#include "cyber/common/log.h"
#include "cyber/init.h"
#include "cyber/mainboard/module_argument.h"
#include "cyber/mainboard/module_controller.h"
#include "cyber/state.h"
using apollo::cyber::mainboard::ModuleArgument;
using apollo::cyber::mainboard::ModuleController;
int main(int argc, char** argv)//命令行参数的数量与内容
{
  //解析参数
  ModuleArgument module_args;
  1.module_args.ParseArgument(argc, argv);//输出解析结果
  //初始化环境
  apollo::cyber::Init(argv[0]); 
  ModuleController controller(module_args);
  //Init中启动LoadAll，若启动失败
  3.if (!controller.Init())
  {
    controller.Clear();
    AERROR << "module start error.";
    return -1;
  }
  //等待关闭
  apollo::cyber::WaitForShutdown();
  //卸载模块
  controller.Clear();
  AINFO << "exit mainboard.";
  return 0;
}

ParseArgument(输出解析信息)

  void ModuleArgument::ParseArgument(const int argc, char* const argv[])
  {
  binary_name_ = std::string(basename(argv[0]));
  //解析命令行参数，根据不同的选项设置相应的变量值，以供程序后续使用(存入dag_conf_list)
  2.GetOptions(argc, argv);
  if (process_group_.empty())
  {
    process_group_ = DEFAULT_process_group_;
  }
  if (sched_name_.empty())
  {
    sched_name_ = DEFAULT_sched_name_;
  }
  //设定process_group和sched_name，若没有则为默认值
  GlobalData::Instance()->SetProcessGroup(process_group_);
  GlobalData::Instance()->SetSchedName(sched_name_);
  AINFO << "binary_name_ is " << binary_name_ << ", process_group_ is "
        << process_group_ << ", has " << dag_conf_list_.size() << " dag conf";
  //从dag_conf_list读取字符串到dag中并打印
  for (std::string& dag : dag_conf_list_)
  {
    AINFO << "dag_conf: " << dag;
  }
}

GetOptions(获取选项设定值)

void ModuleArgument::GetOptions(const int argc, char* const argv[]) {
  opterr = 0;//外部全局变量
  int long_index = 0;
  //定义长短选项，根据输入选择不同操作
  const std::string short_opts = "hd:p:s:";
  static const struct option long_opts[] = {
      {"help", no_argument, nullptr, 'h'},
      {"dag_conf", required_argument, nullptr, 'd'},
      {"process_name", required_argument, nullptr, 'p'},
      {"sched_name", required_argument, nullptr, 's'},
      {NULL, no_argument, nullptr, 0}};
  //命令记录
  std::string cmd("");
  for (int i = 0; i < argc; ++i) {
    cmd += argv[i];
    cmd += " ";
  }
  AINFO << "command: " << cmd;
  if (1 == argc) {
    DisplayUsage();
    exit(0);
  }
  //根据类型执行
  do {
    int opt =getopt_long(argc, argv, short_opts.c_str(), long_opts, &long_index);
    if (opt == -1) {
      break;
    }
    switch (opt) {
      //存储路径
      case 'd':
        dag_conf_list_.emplace_back(std::string(optarg));
        for (int i = optind; i < argc; i++) {
          if (*argv[i] != '-') {
            dag_conf_list_.emplace_back(std::string(argv[i]));
          } else {
            break;
          }
        }
        break;
      case 'p':
        process_group_ = std::string(optarg);
        break;
      case 's':
        sched_name_ = std::string(optarg);
        break;
      case 'h':
        DisplayUsage();
        exit(0);
      default:
        break;
    }
  } while (true);
  if (optind < argc) {
    AINFO << "Found non-option ARGV-element \"" << argv[optind++] << "\"";
    DisplayUsage();
    exit(1);
  }
  if (dag_conf_list_.empty()) {
    AINFO << "-d parameter must be specified";
    DisplayUsage();
    exit(1);
  }
}

LoadAll(加载所有模块)

bool ModuleController::LoadAll()//加载所有模块(生成路径，调用)
{
  //获取根目录
  4.const std::string work_root = common::WorkRoot();
  //获取当前工作目录
  const std::string current_path = common::GetCurrentPath();
  //获取绝对路径
  const std::string dag_root_path = common::GetAbsolutePath(work_root, "dag");
  std::vector<std::string> paths;
  for (auto& dag_conf : args_.GetDAGConfList())
  {
    std::string module_path = "";
    if (dag_conf == common::GetFileName(dag_conf))
    {
    //dag_conf为文件名，则拼接路径
      module_path = common::GetAbsolutePath(dag_root_path, dag_conf);
    } else if (dag_conf[0] == '/')
    {
      //dag_conf为绝对路径
      module_path = dag_conf;
    } else
    {
      //dag_conf为相对路径
      module_path = common::GetAbsolutePath(current_path, dag_conf);
      if (!common::PathExists(module_path))
      {
        module_path = common::GetAbsolutePath(work_root, dag_conf);
      }
    }
    total_component_nums += GetComponentNum(module_path);
    paths.emplace_back(std::move(module_path));
  }
  if (has_timer_component)
  {
    total_component_nums += scheduler::Instance()->TaskPoolSize();
  }
  //将结果转变为全局变量
  common::GlobalData::Instance()->SetComponentNums(total_component_nums);
  for (auto module_path : paths)
  {
    AINFO << "Start initialize dag: " << module_path;
    //加载模块失败
    if (!LoadModule(module_path))
    {
      AERROR << "Failed to load module: " << module_path;
      return false;
    }
  }
  return true;
}

WorkRoot(获取根目录)

inline const std::string WorkRoot()
{
  //检索环境变量CYBER_PATH
  std::string work_root = GetEnv("CYBER_PATH");
  if (work_root.empty())
  {
    work_root = "/apollo/cyber";
  }
  return work_root;
}

Map模块

去年的文件结构如上所示，8.0版本去除了proto部分，所以地图各元素的消息格式暂时未知

Localization模块

Localization模块主要实现车辆的位置（Planning模块）与车辆的姿态、速度信息（Control模块)

输入：IMU与GPS传感器信息

输出:Localization实例

关联模块：Planning+Control

rtk_localization.cc(待补：rtk_localization_component.cc)

void RTKLocalization::GpsCallback(
    const std::shared_ptr<localization::Gps> &gps_msg) {
    //获取延迟
  double time_delay = last_received_timestamp_sec_
                          ? Clock::NowInSeconds() - last_received_timestamp_sec_
                          : last_received_timestamp_sec_;
  //超出时间则提出警告
  if (time_delay > gps_time_delay_tolerance_) {
    std::stringstream ss;
    ss << "GPS message time interval: " << time_delay;
    monitor_logger_.WARN(ss.str());
  }
  //处理错误
  {
    //互斥访问惯性信息
    std::unique_lock<std::mutex> lock(imu_list_mutex_);
    if (imu_list_.empty()) {
      AERROR << "IMU message buffer is empty.";
      if (service_started_) {
        monitor_logger_.ERROR("IMU message buffer is empty.");
      }
      return;
    }
  }
  {
    //互斥访问GPS状态信息
    std::unique_lock<std::mutex> lock(gps_status_list_mutex_);
    if (gps_status_list_.empty()) {
      AERROR << "Gps status message buffer is empty.";
      if (service_started_) {
        monitor_logger_.ERROR("Gps status message buffer is empty.");
      }
      return;
    }
  }
  //发布定位信息
 1. PrepareLocalizationMsg(*gps_msg, &last_localization_result_,&last_localization_status_result_);
  service_started_ = true;
  if (service_started_time == 0.0) {
    service_started_time = Clock::NowInSeconds();
  }
  //监视运行状况
  RunWatchDog(gps_msg->header().timestamp_sec());
  last_received_timestamp_sec_ = Clock::NowInSeconds();
  }

PrepareLocalizationMsg(准备建定位消息)

void RTKLocalization::PrepareLocalizationMsg(const localization::Gps &gps_msg, LocalizationEstimate *localization,LocalizationStatus *localization_status)
{
  //寻找匹配的GPS和IMU消息
  double gps_time_stamp = gps_msg.header().timestamp_sec();
  CorrectedImu imu_msg;
  2.FindMatchingIMU(gps_time_stamp, &imu_msg);
  3.ComposeLocalizationMsg(gps_msg, imu_msg, localization);//组成定位信息
  drivers::gnss::InsStat gps_status;
  FindNearestGpsStatus(gps_time_stamp, &gps_status);//寻找最近的GPS状态
  4.FillLocalizationStatusMsg(gps_status, localization_status);
}

FindMatchingIMU(寻找匹配的IMU)

bool RTKLocalization::FindMatchingIMU(const double gps_timestamp_sec,CorrectedImu *imu_msg) {
  if (imu_msg == nullptr) {
    AERROR << "imu_msg should NOT be nullptr.";
    return false;
  }
  std::unique_lock<std::mutex> lock(imu_list_mutex_);
  auto imu_list = imu_list_;
  lock.unlock();
  if (imu_list.empty()) {
    AERROR << "Cannot find Matching IMU. "<< "IMU message Queue is empty! GPS timestamp[" << gps_timestamp_sec<< "]";
    return false;
  }
  //扫描IMU缓冲区,寻找时间戳更新的消息
  auto imu_it = imu_list.begin();
  for (; imu_it != imu_list.end(); ++imu_it) {
    if ((*imu_it).header().timestamp_sec() - gps_timestamp_sec >std::numeric_limits<double>::min()) {
      break;
    }
  }
  //目标存在
  if (imu_it != imu_list.end()) {
    if (imu_it == imu_list.begin()) {
      AERROR << "IMU queue too short or request too old. "
             << "Oldest timestamp[" << imu_list.front().header().timestamp_sec()
             << "], Newest timestamp["
             << imu_list.back().header().timestamp_sec() << "], GPS timestamp["
             << gps_timestamp_sec << "]";
      *imu_msg = imu_list.front();
    } 
    else {
      //正常情况
      auto imu_it_1 = imu_it;
      imu_it_1--;
      if (!(*imu_it).has_header() || !(*imu_it_1).has_header()) {
        AERROR << "imu1 and imu_it_1 must both have header.";
        return false;
      }
      //插值，同步imu与gps
      if (!InterpolateIMU(*imu_it_1, *imu_it, gps_timestamp_sec, imu_msg)) {
        AERROR << "failed to interpolate IMU";
        return false;
      }
    }
  } 
  else {
    //所有数据时间戳都在请求时间戳之前，则提供最新的IMU数据，不进行外推
    *imu_msg = imu_list.back();
    if (imu_msg == nullptr) {
      AERROR << "Fail to get latest observed imu_msg.";
      return false;
    }
    if (!imu_msg->has_header()) {
      AERROR << "imu_msg must have header.";
      return false;
    }
    if (std::fabs(imu_msg->header().timestamp_sec() - gps_timestamp_sec) > gps_imu_time_diff_threshold_) {
      //报错阈值为20ms
      AERROR << "Cannot find Matching IMU. IMU messages too old. "
             << "Newest timestamp[" << imu_list.back().header().timestamp_sec()
             << "], GPS timestamp[" << gps_timestamp_sec << "]";
    }
  }
  return true;
}

ComposeLocalizationMsg（获取Localization并坐标变换）

void RTKLocalization::ComposeLocalizationMsg(const localization::Gps &gps_msg, const localization::CorrectedImu &imu_msg,LocalizationEstimate *localization) {
  localization->Clear();
  FillLocalizationMsgHeader(localization);
  localization->set_measurement_time(gps_msg.header().timestamp_sec());
  //结合GPS与IMU
  auto mutable_pose = localization->mutable_pose();
  if (gps_msg.has_localization()) {
    const auto &pose = gps_msg.localization();
    if (pose.has_position()) {
      //position为localization的一部分，以下代码可实现world frame -> map frame
      mutable_pose->mutable_position()->set_x(pose.position().x() -map_offset_[0]);
      mutable_pose->mutable_position()->set_y(pose.position().y() -map_offset_[1]);
      mutable_pose->mutable_position()->set_z(pose.position().z() -map_offset_[2]);
    }
    //orientation，四元数转化为航向角
    if (pose.has_orientation()) {
      mutable_pose->mutable_orientation()->CopyFrom(pose.orientation());
      double heading = common::math::QuaternionToHeading(pose.orientation().qw(), pose.orientation().qx(),pose.orientation().qy(), pose.orientation().qz());
      mutable_pose->set_heading(heading);
    }
    //线速度
    if (pose.has_linear_velocity()) {
      mutable_pose->mutable_linear_velocity()->CopyFrom(pose.linear_velocity());
    }
  }
  if (imu_msg.has_imu()) {
    const auto &imu = imu_msg.imu();
    //线加速度
    if (imu.has_linear_acceleration()) {
      if (localization->pose().has_orientation()) {
        //将车辆坐标系中的位置信息转换为地图坐标系中的位置信息
        Vector3d orig(imu.linear_acceleration().x(),imu.linear_acceleration().y(),imu.linear_acceleration().z());
        Vector3d vec = common::math::QuaternionRotate(localization->pose().orientation(), orig);
        mutable_pose->mutable_linear_acceleration()->set_x(vec[0]);
        mutable_pose->mutable_linear_acceleration()->set_y(vec[1]);
        mutable_pose->mutable_linear_acceleration()->set_z(vec[2]);
        //车辆坐标系线加速度
        mutable_pose->mutable_linear_acceleration_vrf()->CopyFrom(imu.linear_acceleration());
      }
      else {
        AERROR << "[PrepareLocalizationMsg]: "<< "fail to convert linear_acceleration";
      }
    }
    //角速度ω
    if (imu.has_angular_velocity()) {
      if (localization->pose().has_orientation()) {
        //convert from vehicle reference to map reference(同上)
        Vector3d orig(imu.angular_velocity().x(), imu.angular_velocity().y(),
                      imu.angular_velocity().z());
        Vector3d vec = common::math::QuaternionRotate(
            localization->pose().orientation(), orig);
        mutable_pose->mutable_angular_velocity()->set_x(vec[0]);
        mutable_pose->mutable_angular_velocity()->set_y(vec[1]);
        mutable_pose->mutable_angular_velocity()->set_z(vec[2]);
        //车辆坐标系角加速度
        mutable_pose->mutable_angular_velocity_vrf()->CopyFrom(
            imu.angular_velocity());
      } else {
        AERROR << "[PrepareLocalizationMsg]: fail to convert angular_velocity";
      }
    }
    //欧拉角
    if (imu.has_euler_angles()) {
      mutable_pose->mutable_euler_angles()->CopyFrom(imu.euler_angles());
    }
  }
}

FillLocalizationStatusMsg(填充Localization中的GNSS状态)

void RTKLocalization::FillLocalizationStatusMsg(const drivers::gnss::InsStat &status,LocalizationStatus *localization_status) {
  apollo::common::Header *header = localization_status->mutable_header();
  double timestamp = apollo::cyber::Clock::NowInSeconds();
  header->set_timestamp_sec(timestamp);
  localization_status->set_measurement_time(status.header().timestamp_sec());
  //检查是否包含位置信息
  if (!status.has_pos_type()) {
    localization_status->set_fusion_status(MeasureState::ERROR);
    localization_status->set_state_message("Error: Current Localization Status Is Missing.");
    return;
  }
  //根据GNSS(GPS)的不同解决方案设置状态
  auto pos_type = static_cast<drivers::gnss::SolutionType>(status.pos_type());
  switch (pos_type) {
    case drivers::gnss::SolutionType::INS_RTKFIXED://差分固定解，亚厘米级精度
      localization_status->set_fusion_status(MeasureState::OK);
      localization_status->set_state_message("");
      break;
    case drivers::gnss::SolutionType::INS_RTKFLOAT://差分浮动解，无法稳定提供差分固定解时的方案
      localization_status->set_fusion_status(MeasureState::WARNNING);
      localization_status->set_state_message("Warning: Current Localization Is Unstable.");
      break;
    default:
      localization_status->set_fusion_status(MeasureState::ERROR);
      localization_status->set_state_message("Error: Current Localization Is Very Unstable.");
      break;
  }
}

Planning模块

Planning模块的作用是根据感知(Prediction)预测的结果，当前的车辆信息和路况规划出一条车辆能够行驶的轨迹，继而交给控制(Control)模块执行

输入：

1. 预测的障碍物信息(prediction_obstacles)
2. 车辆底盘(Chassis)信息(车辆的速度，加速度，航向角等信息)
3. 车辆当前位置(localization_estimate)

输出:规划的路径

关联模块：Control+Localization+Prediction+Routing

planning_component.cc

bool PlanningComponent::Init() {
  //两种模式注册
  injector_ = std::make_shared<DependencyInjector>();
  if (FLAGS_use_navigation_mode) {
    planning_base_ = std::make_unique<NaviPlanning>(injector_);//导航级别路径规划
  } else {
    planning_base_ = std::make_unique<OnLanePlanning>(injector_);//车道级别路径规划
  }
  //配置文件加载
  ACHECK(ComponentBase::GetProtoConfig(&config_))
      << "failed to load planning config file "
      << ComponentBase::ConfigFilePath();
  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模块信息
  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_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);
      });
  //订阅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模块信息
  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);
      });
  //若使用导航级别路径规划，则读取相对地图
  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_writer_ = node_->CreateWriter<ADCTrajectory>(config_.topic_config().planning_trajectory_topic());
  //发布重新Routing请求
  rerouting_writer_ = node_->CreateWriter<RoutingRequest>(config_.topic_config().routing_request_topic());
  //发布规划学习数据（?）
  planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(config_.topic_config().planning_learning_data_topic());
  return true;
}
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);
  //检查并执行可能的重新Routing请求
  CheckRerouting();
  //处理融合后输入数据
  local_view_.prediction_obstacles = prediction_obstacles;
  local_view_.chassis = chassis;
  local_view_.localization_estimate = localization_estimate;
  //更新Routing状态，数据放入local_view
  {
    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_);
  }
  {
    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_);
  }
  if (!CheckInput()) {
    AERROR << "Input check failed";
    return false;
  }
  //在线训练的数据处理
  if (config_.learning_mode() != PlanningConfig::NO_LEARNING) {
    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);
  }
  //发布用于RL测试的数据
  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;
  }
  //执行注册好的Planning，生成线路
  ADCTrajectory adc_trajectory_pb;
  //生成一次路径，将结果存储在adc_trajectory_pb中
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  auto start_time = adc_trajectory_pb.header().timestamp_sec();
  common::util::FillHeader(node_->Name(), &adc_trajectory_pb);
  //根据header时间戳的变化修改轨迹相对时间
  const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
  for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
    p.set_relative_time(p.relative_time() + dt);
  }
  //发布生成的路径
  planning_writer_->Write(adc_trajectory_pb);
  //加入历史记录
  auto* history = injector_->history();
  history->Add(adc_trajectory_pb);
  return true;
}

Control模块

Control模块的作用是根据规划(Planning)模块生成的轨迹，计算出汽车的油门，刹车和方向盘信号，控制汽车按照规定的轨迹行驶

输入：车辆底盘(Chassis)信息, 位置信息(LocalizationEstimate), Planning模块规划的轨迹(ADCTrajectory)

输出：油门、刹车、方向盘控制(ControlCommand)

关联模块：Planning+Localization

control_component.cc

bool ControlComponent::Init() {
  //订阅Chassis信息
  cyber::ReaderConfig chassis_reader_config;
  chassis_reader_config.channel_name = FLAGS_chassis_topic;
  chassis_reader_config.pending_queue_size = FLAGS_chassis_pending_queue_size;
  chassis_reader_ =node_->CreateReader<Chassis>(chassis_reader_config, nullptr);
  ACHECK(chassis_reader_ != nullptr);
  //订阅Planning信息
  cyber::ReaderConfig planning_reader_config;
  planning_reader_config.channel_name = FLAGS_planning_trajectory_topic;
  planning_reader_config.pending_queue_size = FLAGS_planning_pending_queue_size;
  trajectory_reader_ =node_->CreateReader<ADCTrajectory>(planning_reader_config, nullptr);
  ACHECK(trajectory_reader_ != nullptr);
  //订阅Localization信息
  cyber::ReaderConfig localization_reader_config;
  localization_reader_config.channel_name = FLAGS_localization_topic;
  localization_reader_config.pending_queue_size =
      FLAGS_localization_pending_queue_size;
  localization_reader_ = node_->CreateReader<LocalizationEstimate>(localization_reader_config, nullptr);
  ACHECK(localization_reader_ != nullptr);
}
bool ControlComponent::Proc() {
  //读取各模块数据
  const auto start_time = Clock::Now();
  chassis_reader_->Observe();
  const auto &chassis_msg = chassis_reader_->GetLatestObserved();
  if (chassis_msg == nullptr) {
    AERROR << "Chassis msg is not ready!";
    return false;
  }
  OnChassis(chassis_msg);
  trajectory_reader_->Observe();
  const auto &trajectory_msg = trajectory_reader_->GetLatestObserved();
  if (trajectory_msg == nullptr) {
    AERROR << "planning msg is not ready!";
    return false;
  }
  //为什么判断？？？
  if (latest_trajectory_.header().sequence_num() !=trajectory_msg->header().sequence_num()) {
    OnPlanning(trajectory_msg);
  }
  localization_reader_->Observe();
  const auto &localization_msg = localization_reader_->GetLatestObserved();
  if (localization_msg == nullptr) {
    AERROR << "localization msg is not ready!";
    return false;
  }
  OnLocalization(localization_msg);
  pad_msg_reader_->Observe();
  const auto &pad_msg = pad_msg_reader_->GetLatestObserved();
  if (pad_msg != nullptr) {
    OnPad(pad_msg);
  }
/*TODO(SHU):避免冗余拷贝
  {  
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.mutable_chassis()->CopyFrom(latest_chassis_);
    local_view_.mutable_trajectory()->CopyFrom(latest_trajectory_);
    local_view_.mutable_localization()->CopyFrom(latest_localization_);
    if (pad_msg != nullptr) {
      local_view_.mutable_pad_msg()->CopyFrom(pad_msg_);
    }
  }
  //若使用Control子模块
  if (FLAGS_use_control_submodules) {
    local_view_.mutable_header()->set_lidar_timestamp(local_view_.trajectory().header().lidar_timestamp());
    local_view_.mutable_header()->set_camera_timestamp(local_view_.trajectory().header().camera_timestamp());
    local_view_.mutable_header()->set_radar_timestamp(local_view_.trajectory().header().radar_timestamp());
    common::util::FillHeader(FLAGS_control_local_view_topic, &local_view_);
    const auto end_time = Clock::Now();
    //测量延迟
    static apollo::common::LatencyRecorder latency_recorder(FLAGS_control_local_view_topic);
    latency_recorder.AppendLatencyRecord(local_view_.trajectory().header().lidar_timestamp(), start_time,end_time);
    local_view_writer_->Write(local_view_);
    return true;
  }
  //处理pad_msg
  if (pad_msg != nullptr) {
    ADEBUG << "pad_msg: " << pad_msg_.ShortDebugString();
    if (pad_msg_.action() == DrivingAction::RESET) {
      AINFO << "Control received RESET action!";
      estop_ = false;
      estop_reason_.clear();
    }
    pad_received_ = true;
  }
  //结束测试
  if (control_conf_.is_control_test_mode() &&control_conf_.control_test_duration() > 0 &&
      (start_time - init_time_).ToSecond() >control_conf_.control_test_duration()) {
    AERROR << "Control finished testing. exit";
    return false;
  }*/
  //生成控制命令
  ControlCommand control_command;
  Status status;
  if (local_view_.chassis().driving_mode() == apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    1.status = ProduceControlCommand(&control_command);
  } else {
    //重置车辆控制状态
    ResetAndProduceZeroControlCommand(&control_command);
  }
  AERROR_IF(!status.ok()) << "Failed to produce control command:" << status.error_message();
  if (pad_received_) {
    control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
    pad_received_ = false;
  }
  //在Control框架下发布紧急制动原因并设置头部信息
  if (estop_) {
    control_command.mutable_header()->mutable_status()->set_msg(estop_reason_);
  }
  control_command.mutable_header()->set_lidar_timestamp(local_view_.trajectory().header().lidar_timestamp());
  control_command.mutable_header()->set_camera_timestamp(local_view_.trajectory().header().camera_timestamp());
  control_command.mutable_header()->set_radar_timestamp(local_view_.trajectory().header().radar_timestamp());
  common::util::FillHeader(node_->Name(), &control_command);
  ADEBUG << control_command.ShortDebugString();
  //测试模式下不发布控制命令
  if (control_conf_.is_control_test_mode()) {
    ADEBUG << "Skip publish control command in test mode";
    return true;
  }
  //计算时间开销(ms为单位)
  const auto end_time = Clock::Now();
  const double time_diff_ms = (end_time - start_time).ToSecond() * 1e3;
  ADEBUG << "total control time spend: " << time_diff_ms << " ms.";
  control_command.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  control_command.mutable_latency_stats()->set_total_time_exceeded(time_diff_ms > control_conf_.control_period() * 1e3);
  ADEBUG << "control cycle time is: " << time_diff_ms << " ms.";
  status.Save(control_command.mutable_header()->mutable_status());
  //测量延迟
  if (local_view_.trajectory().header().has_lidar_timestamp()) {
    static apollo::common::LatencyRecorder latency_recorder(FLAGS_control_command_topic);
    latency_recorder.AppendLatencyRecord(local_view_.trajectory().header().lidar_timestamp(), start_time, end_time);
  }
  //发布控制命令
  control_cmd_writer_->Write(control_command);
  return true;
}

ProduceControlCommand(计算控制参数并执行)

Status ControlComponent::ProduceControlCommand(ControlCommand *control_command) {
  Status status = CheckInput(&local_view_);
  //检查数据，错误则紧急停车，转换为手动控制
  if (!status.ok()) {
    AERROR_EVERY(100) << "Control input data failed: " << status.error_message();
    control_command->mutable_engage_advice()->set_advice(apollo::common::EngageAdvice::DISALLOW_ENGAGE);
    control_command->mutable_engage_advice()->set_reason(status.error_message());
    estop_ = true;
    estop_reason_ = status.error_message();
  } else {
    //检查时间戳，超时
    Status status_ts = CheckTimestamp(local_view_);
    if (!status_ts.ok()) {
      AERROR << "Input messages timeout";
      //如果没有接收到新的数据，则清空轨迹信息，停止自动控制
      trajectory_reader_->ClearData();
      status = status_ts;
      if (local_view_.chassis().driving_mode() !=apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
        control_command->mutable_engage_advice()->set_advice(apollo::common::EngageAdvice::DISALLOW_ENGAGE);
        control_command->mutable_engage_advice()->set_reason(status.error_message());
      }
    } else {
      control_command->mutable_engage_advice()->set_advice(apollo::common::EngageAdvice::READY_TO_ENGAGE);
    }
  }
  //检查紧急制动
  estop_ = control_conf_.enable_persistent_estop()
               ? estop_ || local_view_.trajectory().estop().is_estop()
               : local_view_.trajectory().estop().is_estop();
  if (local_view_.trajectory().estop().is_estop()) {
    estop_ = true;
    estop_reason_ = "estop from planning : ";
    estop_reason_ += local_view_.trajectory().estop().reason();
  }
  if (local_view_.trajectory().trajectory_point().empty()) {
    AWARN_EVERY(100) << "planning has no trajectory point. ";
    estop_ = true;
    estop_reason_ = "estop for empty planning trajectory, planning headers: " + local_view_.trajectory().header().ShortDebugString();
  }
  //车辆处于驾驶档且速度为负值，则触发
  if (FLAGS_enable_gear_drive_negative_speed_protection) {
    const double kEpsilon = 0.001;
    auto first_trajectory_point = local_view_.trajectory().trajectory_point(0);
    if (local_view_.chassis().gear_location() == Chassis::GEAR_DRIVE &&first_trajectory_point.v() < -1 * kEpsilon) {
      estop_ = true;
      estop_reason_ = "estop for negative speed when gear_drive";
    }
  }
  //手动驾驶状态
  if (!estop_) {
    if (local_view_.chassis().driving_mode() == Chassis::COMPLETE_MANUAL) {
      controller_agent_.Reset();
      AINFO_EVERY(100) << "Reset Controllers in Manual Mode";
    }
    auto debug = control_command->mutable_debug()->mutable_input_debug();
    debug->mutable_localization_header()->CopyFrom(local_view_.localization().header());
    debug->mutable_canbus_header()->CopyFrom(local_view_.chassis().header());
    debug->mutable_trajectory_header()->CopyFrom(local_view_.trajectory().header());
    if (local_view_.trajectory().is_replan()) {
      latest_replan_trajectory_header_ = local_view_.trajectory().header();
    }
    //重新规划时记录头部文件
    if (latest_replan_trajectory_header_.has_sequence_num()) {
      debug->mutable_latest_replan_trajectory_header()->CopyFrom(
          latest_replan_trajectory_header_);
    }
    //控制器代理(核心部分)，利用传入的Localization、chassis、trajectory信息计算控制参数
    Status status_compute = controller_agent_.ComputeControlCommand(&local_view_.localization(), &local_view_.chassis(),&local_view_.trajectory(), control_command);
    if (!status_compute.ok()) {
      AERROR << "Control main function failed"
             << " with localization: "
             << local_view_.localization().ShortDebugString()
             << " with chassis: " << local_view_.chassis().ShortDebugString()
             << " with trajectory: "
             << local_view_.trajectory().ShortDebugString()
             << " with cmd: " << control_command->ShortDebugString()
             << " status:" << status_compute.error_message();
      estop_ = true;
      estop_reason_ = status_compute.error_message();
      status = status_compute;
    }
  }
  //如果Planning模块触发了紧急制动,则不会执行Control命令
  if (estop_) {
    AWARN_EVERY(100) << "Estop triggered! No control core method executed!";
    control_command->set_speed(0);
    control_command->set_throttle(0);
    control_command->set_brake(control_conf_.soft_estop_brake());
    control_command->set_gear_location(Chassis::GEAR_DRIVE);
  }
  //将signal信息(Planning？)复制到Control模块
  if (local_view_.trajectory().decision().has_vehicle_signal()) {
    control_command->mutable_signal()->CopyFrom(local_view_.trajectory().decision().vehicle_signal());
  }
  return status;
}

pad_msg主要包括以下两种功能：

1. 发送消息给Canbus模块，来控制driving_mode，Control模块根据当前driving_mode的状态来判断是否启动自动驾驶(pad_msg由Planning模块订阅并发布)
2. 通过reset来清空estop_的状态(estop_其中一个产生条件为路径为空，起源于Planning模块)

Perception模块

Perception模块利用各种传感器数据中获取的有关环境信息，使得车辆能够理解、感知其周围的事物

输入：雷达与图像数据，车辆的速度与角速度

输出：具有航向、速度和分类信息的3D障碍物轨迹以及交通信号灯检测和识别的输出

关联模块：Prediction+Localization

Prediction模块

Prediction模块主要研究并预测感知模块(Perception)检测到的所有障碍物的行为，预测接收障碍物数据以及基本感知信息，包括位置、航向、速度、加速度，然后生成具有这些障碍物概率的预测轨迹

输入：感知模块的障碍物信息(Perception)，定位模块的定位信息(Localization)，规划模块的前一个计算周期的规划轨迹(Planning)

输出：具有预测轨迹的障碍物，障碍物标注有预测轨迹及其优先级

关联模块：Perception+Localization+Planning*

prediction_component.cc

void PredictionComponent::OfflineProcessFeatureProtoFile(const std::string& features_proto_file_name) {
  //读取障碍物容器指针
  auto obstacles_container_ptr = container_manager_->GetContainer<ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
  obstacles_container_ptr->Clear();
  //将特征加入障碍物容器(Perception)
  Features features;
  apollo::cyber::common::GetProtoFromBinaryFile(features_proto_file_name,&features);
  for (const Feature& feature : features.feature()) {
    obstacles_container_ptr->InsertFeatureProto(feature);
    Obstacle* obstacle_ptr = obstacles_container_ptr->GetObstacle(feature.id());
    if (!obstacle_ptr) {
      continue;
    }
    //评估障碍物?
    1.evaluator_manager_->EvaluateObstacle(obstacle_ptr, obstacles_container_ptr);
  }
}
bool PredictionComponent::Init() {
  component_start_time_ = Clock::NowInSeconds();
  container_manager_ = std::make_shared<ContainerManager>();
  evaluator_manager_.reset(new EvaluatorManager());
  predictor_manager_.reset(new PredictorManager());
  scenario_manager_.reset(new ScenarioManager());
  PredictionConf prediction_conf;
  //未能加载配置文件
  if (!ComponentBase::GetProtoConfig(&prediction_conf)) {
    AERROR << "Unable to load prediction conf file: " << ComponentBase::ConfigFilePath();
    return false;
  }
  ADEBUG << "Prediction config file is loaded into: " << prediction_conf.ShortDebugString();
  if (!MessageProcess::Init(container_manager_.get(), evaluator_manager_.get(), predictor_manager_.get(), prediction_conf)) {
    return false;
  }
  //订阅各模块信息
  planning_reader_ = node_->CreateReader<ADCTrajectory>(prediction_conf.topic_conf().planning_trajectory_topic(), nullptr);
  localization_reader_ = node_->CreateReader<localization::LocalizationEstimate>(prediction_conf.topic_conf().localization_topic(), nullptr);
  storytelling_reader_ = node_->CreateReader<storytelling::Stories>(prediction_conf.topic_conf().storytelling_topic(), nullptr);
  prediction_writer_ = node_->CreateWriter<PredictionObstacles>(prediction_conf.topic_conf().prediction_topic());
  container_writer_ = node_->CreateWriter<SubmoduleOutput>(prediction_conf.topic_conf().container_topic_name());
  adc_container_writer_ = node_->CreateWriter<ADCTrajectoryContainer>(prediction_conf.topic_conf().adccontainer_topic_name());
  perception_obstacles_writer_ = node_->CreateWriter<PerceptionObstacles>(prediction_conf.topic_conf().perception_obstacles_topic_name());
  return true;
}
bool PredictionComponent::Proc(const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
  if (FLAGS_use_lego) {
    //将各模块内容处理后，放入容器供其他部分使用
    3.return ContainerSubmoduleProcess(perception_obstacles);
  }
  //涉及更多的数据处理和后续任务，发布预测的障碍物信息
  4.return PredictionEndToEndProc(perception_obstacles);
}

EvaluateObstacle(根据障碍物选择评估器)

void EvaluatorManager::EvaluateObstacle(const ADCTrajectoryContainer* adc_trajectory_container,
    Obstacle* obstacle,ObstaclesContainer* obstacles_container,std::vector<Obstacle*> dynamic_env) {
  Evaluator* evaluator = nullptr;
  //根据障碍物的类型选择不同的评估器(evaluators)
  switch (obstacle->type()) {
    case PerceptionObstacle::VEHICLE: {
      //警告却不是慢行(潜在危险)
      if (obstacle->IsCaution() && !obstacle->IsSlow()) {
        if (obstacle->IsInteractiveObstacle()) {
          evaluator = GetEvaluator(interaction_evaluator_);
        } else if (obstacle->IsNearJunction()) {
          evaluator = GetEvaluator(vehicle_in_junction_caution_evaluator_);
        } else if (obstacle->IsOnLane()) {
          evaluator = GetEvaluator(vehicle_on_lane_caution_evaluator_);
        } else {
          evaluator = GetEvaluator(vehicle_default_caution_evaluator_);
        }
        CHECK_NOTNULL(evaluator);
        //评估并退出
        if (evaluator->GetName() == "JOINTLY_PREDICTION_PLANNING_EVALUATOR") {
          2.if (evaluator->Evaluate(adc_trajectory_container,obstacle, obstacles_container)) {
            break;
          } else {
            AERROR << "Obstacle: " << obstacle->id() << " interaction evaluator failed," << " downgrade to normal level!";
          }
        } else {
          if (evaluator->Evaluate(obstacle, obstacles_container)) {
            break;
          } else {
            AERROR << "Obstacle: " << obstacle->id() << " caution evaluator failed, downgrade to normal level!";
          }
        }
      }
      //障碍物不属于警告类别或警告评估失败
      if (obstacle->HasJunctionFeatureWithExits() &&!obstacle->IsCloseToJunctionExit()) {
        //障碍物在交叉路口里
        evaluator = GetEvaluator(vehicle_in_junction_evaluator_);
      } else if (obstacle->IsOnLane()) {
        evaluator = GetEvaluator(vehicle_on_lane_evaluator_);
      } else {
        //不再评估并退出
        ADEBUG << "Obstacle: " << obstacle->id() << " is neither on lane, nor in junction. Skip evaluating.";
        break;
      }
      CHECK_NOTNULL(evaluator);
      if (evaluator->GetName() == "LANE_SCANNING_EVALUATOR") {
        evaluator->Evaluate(obstacle, obstacles_container, dynamic_env);
      } else {
        evaluator->Evaluate(obstacle, obstacles_container);
      }
      break;
    }
    case PerceptionObstacle::BICYCLE: {
      if (obstacle->IsOnLane()) {
        evaluator = GetEvaluator(cyclist_on_lane_evaluator_);
        CHECK_NOTNULL(evaluator);
        evaluator->Evaluate(obstacle, obstacles_container);
      }
      break;
    }
    case PerceptionObstacle::PEDESTRIAN: {
      //若处于离线模式或者优先级为CAUTION，则进行评估
      if (FLAGS_prediction_offline_mode == PredictionConstants::kDumpDataForLearning ||
          obstacle->latest_feature().priority().priority() == ObstaclePriority::CAUTION) {
        evaluator = GetEvaluator(pedestrian_evaluator_);
        CHECK_NOTNULL(evaluator);
        evaluator->Evaluate(obstacle, obstacles_container);
        break;
      }
    }
    default: {
      if (obstacle->IsOnLane()) {
        evaluator = GetEvaluator(default_on_lane_evaluator_);
        CHECK_NOTNULL(evaluator);
        evaluator->Evaluate(obstacle, obstacles_container);
      }
      break;
    }
  }
}

PedestrianInteractionEvaluator::Evaluate(使用深度学习模型进行预测和评估)

bool PedestrianInteractionEvaluator::Evaluate(Obstacle* obstacle_ptr, ObstaclesContainer* obstacles_container) {
  //检查健全性(初始化)
  CHECK_NOTNULL(obstacle_ptr);
  obstacle_ptr->SetEvaluatorType(evaluator_type_);
  int id = obstacle_ptr->id();
  if (!obstacle_ptr->latest_feature().IsInitialized()) {
    AERROR << "Obstacle [" << id << "] has no latest feature.";
    return false;
  }
  Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
  CHECK_NOTNULL(latest_feature_ptr);
  //提取障碍物的特征信息并根据不同模式处理
  //-离线模式, 在本地保存以供训练
  //-在线模式, 通过训练模型进行评估
  std::vector<double> feature_values;
  ExtractFeatures(obstacle_ptr, &feature_values);
  if (FLAGS_prediction_offline_mode == PredictionConstants::kDumpDataForLearning) {
    FeatureOutput::InsertDataForLearning(*latest_feature_ptr, feature_values, "pedestrian", nullptr);
    ADEBUG << "Saving extracted features for learning locally.";
    return true;
  }
  static constexpr double kShortTermPredictionTimeResolution = 0.4;
  static constexpr int kShortTermPredictionPointNum = 5;
  static constexpr int kHiddenStateUpdateCycle = 4;
  //Step 1 Get social embedding
  torch::Tensor social_pooling = GetSocialPooling();
  std::vector<torch::jit::IValue> social_embedding_inputs;
  social_embedding_inputs.push_back(std::move(social_pooling.to(device_)));
  torch::Tensor social_embedding =
      torch_social_embedding_.forward(social_embedding_inputs)
          .toTensor()
          .to(torch::kCPU);

  // Step 2 Get position embedding
  double pos_x = feature_values[2];
  double pos_y = feature_values[3];
  double rel_x = 0.0;
  double rel_y = 0.0;
  if (obstacle_ptr->history_size() > kHiddenStateUpdateCycle - 1) {
    rel_x = obstacle_ptr->latest_feature().position().x() - obstacle_ptr->feature(3).position().x();
    rel_y = obstacle_ptr->latest_feature().position().y() - obstacle_ptr->feature(3).position().y();
  }
  torch::Tensor torch_position = torch::zeros({1, 2});
  torch_position[0][0] = rel_x;
  torch_position[0][1] = rel_y;
  std::vector<torch::jit::IValue> position_embedding_inputs;
  position_embedding_inputs.push_back(std::move(torch_position.to(device_)));
  torch::Tensor position_embedding = torch_position_embedding_.forward(position_embedding_inputs).toTensor().to(torch::kCPU);
  // Step 3 Conduct single LSTM and update hidden states
  torch::Tensor lstm_input =
      torch::zeros({1, 2 * (kEmbeddingSize + kHiddenSize)});
  for (int i = 0; i < kEmbeddingSize; ++i) {
    lstm_input[0][i] = position_embedding[0][i];
  }
  if (obstacle_id_lstm_state_map_.find(id) == obstacle_id_lstm_state_map_.end()) {
    obstacle_id_lstm_state_map_[id].ht = torch::zeros({1, 1, kHiddenSize});
    obstacle_id_lstm_state_map_[id].ct = torch::zeros({1, 1, kHiddenSize});
    obstacle_id_lstm_state_map_[id].timestamp = obstacle_ptr->timestamp();
    obstacle_id_lstm_state_map_[id].frame_count = 0;
  }
  torch::Tensor curr_ht = obstacle_id_lstm_state_map_[id].ht;
  torch::Tensor curr_ct = obstacle_id_lstm_state_map_[id].ct;
  int curr_frame_count = obstacle_id_lstm_state_map_[id].frame_count;
  if (curr_frame_count == kHiddenStateUpdateCycle - 1) {
    for (int i = 0; i < kHiddenSize; ++i) {
      lstm_input[0][kEmbeddingSize + i] = curr_ht[0][0][i];
      lstm_input[0][kEmbeddingSize + kHiddenSize + i] = curr_ct[0][0][i];
    }
    std::vector<torch::jit::IValue> lstm_inputs;
    lstm_inputs.push_back(std::move(lstm_input.to(device_)));
    auto lstm_out_tuple = torch_single_lstm_.forward(lstm_inputs).toTuple();
    auto ht = lstm_out_tuple->elements()[0].toTensor();
    auto ct = lstm_out_tuple->elements()[1].toTensor();
    obstacle_id_lstm_state_map_[id].ht = ht.clone();
    obstacle_id_lstm_state_map_[id].ct = ct.clone();
  }
  obstacle_id_lstm_state_map_[id].frame_count = (curr_frame_count + 1) % kHiddenStateUpdateCycle;
  //Step 4 for-loop get a trajectory
  //Set the starting trajectory point
  Trajectory* trajectory = latest_feature_ptr->add_predicted_trajectory();
  trajectory->set_probability(1.0);
  TrajectoryPoint* start_point = trajectory->add_trajectory_point();
  start_point->mutable_path_point()->set_x(pos_x);
  start_point->mutable_path_point()->set_y(pos_y);
  start_point->mutable_path_point()->set_theta(latest_feature_ptr->theta());
  start_point->set_v(latest_feature_ptr->speed());
  start_point->set_relative_time(0.0);
  for (int i = 1; i <= kShortTermPredictionPointNum; ++i) {
    double prev_x = trajectory->trajectory_point(i - 1).path_point().x();
    double prev_y = trajectory->trajectory_point(i - 1).path_point().y();
    ACHECK(obstacle_id_lstm_state_map_.find(id) !=
           obstacle_id_lstm_state_map_.end());
    torch::Tensor torch_position = torch::zeros({1, 2});
    double curr_rel_x = rel_x;
    double curr_rel_y = rel_y;
    if (i > 1) {
      curr_rel_x =
          prev_x - trajectory->trajectory_point(i - 2).path_point().x();
      curr_rel_y =
          prev_y - trajectory->trajectory_point(i - 2).path_point().y();
    }
    torch_position[0][0] = curr_rel_x;
    torch_position[0][1] = curr_rel_y;
    std::vector<torch::jit::IValue> position_embedding_inputs;
    position_embedding_inputs.push_back(std::move(torch_position.to(device_)));
    torch::Tensor position_embedding = torch_position_embedding_.forward(position_embedding_inputs).toTensor().to(torch::kCPU);
    torch::Tensor lstm_input = torch::zeros({1, kEmbeddingSize + 2 * kHiddenSize});
    for (int i = 0; i < kEmbeddingSize; ++i) {
      lstm_input[0][i] = position_embedding[0][i];
    }
    auto ht = obstacle_id_lstm_state_map_[id].ht.clone();
    auto ct = obstacle_id_lstm_state_map_[id].ct.clone();
    for (int i = 0; i < kHiddenSize; ++i) {
      lstm_input[0][kEmbeddingSize + i] = ht[0][0][i];
      lstm_input[0][kEmbeddingSize + kHiddenSize + i] = ct[0][0][i];
    }
    std::vector<torch::jit::IValue> lstm_inputs;
    lstm_inputs.push_back(std::move(lstm_input.to(device_)));
    auto lstm_out_tuple = torch_single_lstm_.forward(lstm_inputs).toTuple();
    ht = lstm_out_tuple->elements()[0].toTensor();
    ct = lstm_out_tuple->elements()[1].toTensor();
    std::vector<torch::jit::IValue> prediction_inputs;
    prediction_inputs.push_back(ht[0]);
    auto pred_out_tensor = torch_prediction_layer_.forward(prediction_inputs).toTensor().to(torch::kCPU);
    auto pred_out = pred_out_tensor.accessor<float, 2>();
    TrajectoryPoint* point = trajectory->add_trajectory_point();
    double curr_x = prev_x + static_cast<double>(pred_out[0][0]);
    double curr_y = prev_y + static_cast<double>(pred_out[0][1]);
    point->mutable_path_point()->set_x(curr_x);
    point->mutable_path_point()->set_y(curr_y);
    point->set_v(latest_feature_ptr->speed());
    point->mutable_path_point()->set_theta(latest_feature_ptr->velocity_heading());
    point->set_relative_time(kShortTermPredictionTimeResolution * static_cast<double>(i));
  }
  return true;
}

ContainerSubmoduleProcess(处理各模块信息并更新发布)

bool PredictionComponent::ContainerSubmoduleProcess(const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
  constexpr static size_t kHistorySize = 10;
  const auto frame_start_time = Clock::Now();
  //读取Localization信息，并且调用OnLocalization更新
  localization_reader_->Observe();
  auto ptr_localization_msg = localization_reader_->GetLatestObserved();
  if (ptr_localization_msg == nullptr) {
    AERROR << "Prediction: cannot receive any localization message.";
    return false;
  }
  MessageProcess::OnLocalization(container_manager_.get(),*ptr_localization_msg);
  //读取Planning上一时刻的信息
  planning_reader_->Observe();
  auto ptr_trajectory_msg = planning_reader_->GetLatestObserved();
  if (ptr_trajectory_msg != nullptr) {
    MessageProcess::OnPlanning(container_manager_.get(), *ptr_trajectory_msg);
  }
  //同上
  storytelling_reader_->Observe();
  auto ptr_storytelling_msg = storytelling_reader_->GetLatestObserved();
  if (ptr_storytelling_msg != nullptr) {
    MessageProcess::OnStoryTelling(container_manager_.get(),*ptr_storytelling_msg);
  }
  =====================================================================================================================
  MessageProcess::ContainerProcess(container_manager_, *perception_obstacles,scenario_manager_.get());
  auto obstacles_container_ptr = container_manager_->GetContainer<ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
  CHECK_NOTNULL(obstacles_container_ptr);
  auto adc_trajectory_container_ptr = container_manager_->GetContainer<ADCTrajectoryContainer>(AdapterConfig::PLANNING_TRAJECTORY);
  CHECK_NOTNULL(adc_trajectory_container_ptr);
  //获取子模块输出
  SubmoduleOutput submodule_output = obstacles_container_ptr->GetSubmoduleOutput(kHistorySize,frame_start_time);
  submodule_output.set_curr_scenario(scenario_manager_->scenario());
  container_writer_->Write(submodule_output);
  adc_container_writer_->Write(*adc_trajectory_container_ptr);
  perception_obstacles_writer_->Write(*perception_obstacles);
  return true;
}

PredictionEndToEndProc

bool PredictionComponent::PredictionEndToEndProc(const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
  //测试模式
  if (FLAGS_prediction_test_mode && (Clock::NowInSeconds() - component_start_time_ > FLAGS_prediction_test_duration)) {
    ADEBUG << "Prediction finished running in test mode";
  }
  //检查相对地图(实时)
  if (FLAGS_use_navigation_mode && !PredictionMap::Ready()) {
    AERROR << "Relative map is empty.";
    return false;
  }
  frame_start_time_ = Clock::NowInSeconds();
  auto end_time1 = std::chrono::system_clock::now();
  //读取Localization信息并更新
  localization_reader_->Observe();
  auto ptr_localization_msg = localization_reader_->GetLatestObserved();
  if (ptr_localization_msg == nullptr) {
    AERROR << "Prediction: cannot receive any localization message.";
    return false;
  }
  MessageProcess::OnLocalization(container_manager_.get(),*ptr_localization_msg);
  auto end_time2 = std::chrono::system_clock::now();
  std::chrono::duration<double> diff = end_time2 - end_time1;
  ADEBUG << "Time for updating PoseContainer: " << diff.count() * 1000 << " msec.";
  //读取Storytelling消息并更新
  storytelling_reader_->Observe();
  auto ptr_storytelling_msg = storytelling_reader_->GetLatestObserved();
  if (ptr_storytelling_msg != nullptr) {
    MessageProcess::OnStoryTelling(container_manager_.get(),*ptr_storytelling_msg);
  }
  planning_reader_->Observe();
  auto ptr_trajectory_msg = planning_reader_->GetLatestObserved();
  if (ptr_trajectory_msg != nullptr) {
    MessageProcess::OnPlanning(container_manager_.get(), *ptr_trajectory_msg);
  }
  auto end_time3 = std::chrono::system_clock::now();
  diff = end_time3 - end_time2;
  ADEBUG << "Time for updating ADCTrajectoryContainer: " << diff.count() * 1000 << " msec.";
  =====================================================================================================================
  //获取所有perception_obstacles，并调用OnPerception进行处理
  auto perception_msg = *perception_obstacles;
  PredictionObstacles prediction_obstacles;
  //将Perception模块内容转换为Prediction模块可用的格式
  MessageProcess::OnPerception(perception_msg, container_manager_, evaluator_manager_.get(),predictor_manager_.get(), scenario_manager_.get(), &prediction_obstacles);
  auto end_time4 = std::chrono::system_clock::now();
  diff = end_time4 - end_time3;
  ADEBUG << "Time for updating PerceptionContainer: " << diff.count() * 1000 << " msec.";
  //对预测的障碍物信息进行后续处理
  prediction_obstacles.set_start_timestamp(frame_start_time_);
  prediction_obstacles.set_end_timestamp(Clock::NowInSeconds());
  prediction_obstacles.mutable_header()->set_lidar_timestamp(perception_msg.header().lidar_timestamp());
  prediction_obstacles.mutable_header()->set_camera_timestamp(perception_msg.header().camera_timestamp());
  prediction_obstacles.mutable_header()->set_radar_timestamp(perception_msg.header().radar_timestamp());
  prediction_obstacles.set_perception_error_code(perception_msg.error_code());
  //循环遍历所有trajectory_point，检查合法性(预测的轨迹可能存在问题)
  if (FLAGS_prediction_test_mode) {
    for (auto const& prediction_obstacle :
         prediction_obstacles.prediction_obstacle()) {
      for (auto const& trajectory : prediction_obstacle.trajectory()) {
        for (auto const& trajectory_point : trajectory.trajectory_point()) {
          if (!ValidationChecker::ValidTrajectoryPoint(trajectory_point)) {
            AERROR << "Invalid trajectory point [" << trajectory_point.ShortDebugString() << "]";
            break;
          }
        }
      }
    }
  }
  auto end_time5 = std::chrono::system_clock::now();
  diff = end_time5 - end_time1;
  ADEBUG << "End to end time elapsed: " << diff.count() * 1000 << " msec.";
  //发布prediction_obstacles
  common::util::FillHeader(node_->Name(), &prediction_obstacles);
  prediction_writer_->Write(prediction_obstacles);
  return true;
}

容器存储来自订阅通道的输入数据
评估器对任何给定的障碍分别预测路径和速度
预测器生成障碍物的预测轨迹

Routing模块

Routing模块主要关注起点到终点的长期路径，需要根据起点到终点之间的道路，选择一条最优路径

输入：地图数据(Map)和路由请求(routing_request)

输出：规划的路径

关联模块：Map

routing_component.cc

#include "modules/routing/routing_component.h"
#include <utility>
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/routing/common/routing_gflags.h"
DECLARE_string(flagfile);
namespace apollo {
namespace routing {
bool RoutingComponent::Init() {
  //加载配置文件
  RoutingConfig routing_conf;
  ACHECK(cyber::ComponentBase::GetProtoConfig(&routing_conf))
      << "Unable to load routing conf file: "
      << cyber::ComponentBase::ConfigFilePath();
  AINFO << "Config file: " << cyber::ComponentBase::ConfigFilePath() << " is loaded.";
  //配置QOS，
  apollo::cyber::proto::RoleAttributes attr;
  attr.set_channel_name(routing_conf.topic_config().routing_response_topic());
  auto qos = attr.mutable_qos_profile();
  //保持最新历史
  qos->set_history(apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
  //要求可靠传输
  qos->set_reliability(apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
  //消息在本例持久化(断线依旧可用)
  qos->set_durability(apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
  //创建消息发布
  response_writer_ = node_->CreateWriter<RoutingResponse>(attr);
  apollo::cyber::proto::RoleAttributes attr_history;
  attr_history.set_channel_name(routing_conf.topic_config().routing_response_history_topic());
  auto qos_history = attr_history.mutable_qos_profile();
  qos_history->set_history(apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
  qos_history->set_reliability(apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
  qos_history->set_durability(apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
  response_history_writer_ = node_->CreateWriter<RoutingResponse>(attr_history);
  //创建定时器，按间隔更新时间戳
  std::weak_ptr<RoutingComponent> self = std::dynamic_pointer_cast<RoutingComponent>(shared_from_this());
  timer_.reset(new ::apollo::cyber::Timer(
      FLAGS_routing_response_history_interval_ms,
      [self, this]() {
        auto ptr = self.lock();
        if (ptr) {
          std::lock_guard<std::mutex> guard(this->mutex_);
          if (this->response_ != nullptr) {
            auto timestamp = apollo::cyber::Clock::NowInSeconds();
            response_->mutable_header()->set_timestamp_sec(timestamp);
            //记录历史
            this->response_history_writer_->Write(*response_);
          }
        }
      },
      false));
  timer_->Start();
  //初始化并启动
  1.return routing_.Init().ok() && routing_.Start().ok();
}
//收到路由请求时触发
bool RoutingComponent::Proc(const std::shared_ptr<RoutingRequest>& request) {
  auto response = std::make_shared<RoutingResponse>();
  //处理Routing请求
  2.if (!routing_.Process(request, response.get())) {
    return false;
  }
  //填充头部并发布响应(协作)
  common::util::FillHeader(node_->Name(), response.get());
  response_writer_->Write(response);
  {
    std::lock_guard<std::mutex> guard(mutex_);
    response_ = std::move(response);
  }
  return true;
}
}
}

routing.cc

#include "modules/routing/routing.h"
#include <limits>
#include <unordered_map>
#include "modules/common/util/point_factory.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/routing/common/routing_gflags.h"
namespace apollo {
namespace routing {
using apollo::common::ErrorCode;
using apollo::common::PointENU;
using apollo::hdmap::ParkingSpaceInfoConstPtr;
apollo::common::Status Routing::Init() {
  //读取地图文件(RoutingMap主要由点和边组成)
  const auto routing_map_file = apollo::hdmap::RoutingMapFile();
  AINFO << "Use routing topology graph path: " << routing_map_file;
  navigator_ptr_.reset(new Navigator(routing_map_file));
  //加载BaseMap
  hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
  ACHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();
  return apollo::common::Status::OK();
}
apollo::common::Status Routing::Start() {
  if (!navigator_ptr_->IsReady()) {
    AERROR << "Navigator is not ready!";
    return apollo::common::Status(ErrorCode::ROUTING_ERROR,"Navigator not ready");
  }
  //发布Routing状态
  AINFO << "Routing service is ready.";
  monitor_logger_buffer_.INFO("Routing started");
  return apollo::common::Status::OK();
}
bool Routing::Process(const std::shared_ptr<RoutingRequest>& routing_request,RoutingResponse* const routing_response) {
  CHECK_NOTNULL(routing_response);
  AINFO << "Get new routing request:" << routing_request->DebugString();
  //对车道信息进行填充(寻找最近车道)
  1.const auto& fixed_requests = FillLaneInfoIfMissing(*routing_request);
  double min_routing_length = std::numeric_limits<double>::max();
  //从所有额外路径中挑选最优的
  for (const auto& fixed_request : fixed_requests) {
    RoutingResponse routing_response_temp;
    if (navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)) {
      const double routing_length = routing_response_temp.measurement().distance();
      if (routing_length < min_routing_length) {
        routing_response->CopyFrom(routing_response_temp);
        min_routing_length = routing_length;
      }
    }
    //对停车信息进行填充
    FillParkingID(routing_response);
  }
  //寻找到合适路径，停车位置规划完毕
  if (min_routing_length < std::numeric_limits<double>::max() && SupplementParkingRequest(routing_response)) {
    monitor_logger_buffer_.INFO("Routing success!");
    return true;
  }
  AERROR << "Failed to search route with navigator.";
  monitor_logger_buffer_.WARN("Routing failed! " + routing_response->status().msg());
  return false;
}
}
}

FillLaneInfoIfMissing(寻找距离请求点最近的车道)

std::vector<RoutingRequest> Routing::FillLaneInfoIfMissing(const RoutingRequest& routing_request) {
  std::vector<RoutingRequest> fixed_requests;
  std::unordered_map<int, std::vector<LaneWaypoint>>additional_lane_waypoint_map;
  RoutingRequest fixed_request(routing_request);
  for (int i = 0; i < routing_request.waypoint_size(); ++i) {
    LaneWaypoint lane_waypoint(routing_request.waypoint(i));
    if (lane_waypoint.has_id()) {
      continue;
    }
    //填充缺少的车道信息
    const auto point =common::util::PointFactory::ToPointENU(lane_waypoint.pose());//将pose信息转换为坐标点
    std::vector<std::shared_ptr<const hdmap::LaneInfo>> lanes;
    //在没有找到车道时扩大半径，寻找最近的车道
    constexpr double kRadius = 0.3;
    for (int i = 0; i < 20; ++i) {
      hdmap_->GetLanes(point, kRadius + i * kRadius, &lanes);
      if (lanes.size() > 0) {
        break;
      }
    }
    if (lanes.empty()) {
      AERROR << "Failed to find nearest lane from map at position: " << point.DebugString();
      return fixed_requests;//返回空的Vector
    }
    for (size_t j = 0; j < lanes.size(); ++j) {
      double s = 0.0;
      double l = 0.0;
      lanes[j]->GetProjection({point.x(), point.y()}, &s, &l);
      if (j == 0) {
        auto waypoint_info = fixed_request.mutable_waypoint(i);
        waypoint_info->set_id(lanes[j]->id().id());
        waypoint_info->set_s(s);
      } else {
        //额外的备选车道
        LaneWaypoint new_lane_waypoint(lane_waypoint);
        new_lane_waypoint.set_id(lanes[j]->id().id());
        new_lane_waypoint.set_s(s);
        additional_lane_waypoint_map[i].push_back(new_lane_waypoint);
      }
    }
  }
  //第一个处理后的路由请求
  fixed_requests.push_back(fixed_request);
  //由于车道重叠生成额外路由请求
  for (const auto& m : additional_lane_waypoint_map) {
    size_t cur_size = fixed_requests.size();
    for (size_t i = 0; i < cur_size; ++i) {
      //使用index迭代的同时push_back
      for (const auto& lane_waypoint : m.second) {
        RoutingRequest new_request(fixed_requests[i]);
        auto waypoint_info = new_request.mutable_waypoint(m.first);
        waypoint_info->set_id(lane_waypoint.id());
        waypoint_info->set_s(lane_waypoint.s());
        fixed_requests.push_back(new_request);
      }
    }
  }//在主要路由请求的基础上，将备选车道转换为额外路由请求，应对车道重叠的情况(为路径规划器提供更多选择)
  for (const auto& fixed_request : fixed_requests) {
    ADEBUG << "Fixed routing request:" << fixed_request.DebugString();
  }
  return fixed_requests;
}

Canbus模块

Canbus模块可以利用订阅的信息，向车辆控制系统传递命令以达到控制底盘(Chassis)的目的，同时可以实时进行车辆数据的接受与发送

输入：控制命令(Control Command)+保护命令(Guardian Command)

输出：汽车底盘信息(Chassis)

关联模块：Control+Guardian

canbus_component.cc

void CanbusComponent::Clear() {
  vehicle_object_->Stop();
  AINFO << "Cleanup Canbus component";
}
bool CanbusComponent::Init() {
  //加载配置文件
  if (!GetProtoConfig(&canbus_conf_)) {
    AERROR << "Unable to load canbus conf file: " << ConfigFilePath();
    return false;
  }
  AINFO << "The canbus conf file is loaded: " << FLAGS_canbus_conf_file;
  ADEBUG << "Canbus_conf:" << canbus_conf_.ShortDebugString();
  //加载车辆工厂库
  if (!apollo::cyber::common::PathExists(FLAGS_load_vehicle_library)) {
    AERROR << FLAGS_load_vehicle_library << " No such vehicle library";
    return false;
  }
  AINFO << "Load the vehicle factory library: " << FLAGS_load_vehicle_library;
  //创建车辆对象
  ClassLoader loader(FLAGS_load_vehicle_library);
  auto vehicle_object = loader.CreateClassObj<AbstractVehicleFactory>(FLAGS_load_vehicle_class_name);
  if (!vehicle_object) {
    AERROR << "Failed to create the vehicle factory: " << FLAGS_load_vehicle_class_name;
    return false;
  }
  vehicle_object_ = vehicle_object;
  if (vehicle_object_ == nullptr) {
    AERROR << "Failed to create vehicle factory pointer.";
  }
  AINFO << "Successfully create vehicle factory: " << FLAGS_load_vehicle_class_name;
  //初始化车辆对象并传入配置信息
  if (!vehicle_object_->Init(&canbus_conf_)) {
    AERROR << "Fail to init vehicle factory.";
    return false;
  }
  AINFO << "Vehicle factory is successfully initialized.";
  //配置消息订阅器(guardian+control)
  cyber::ReaderConfig guardian_cmd_reader_config;
  guardian_cmd_reader_config.channel_name = FLAGS_guardian_topic;
  guardian_cmd_reader_config.pending_queue_size = FLAGS_guardian_cmd_pending_queue_size;
  cyber::ReaderConfig control_cmd_reader_config;
  control_cmd_reader_config.channel_name = FLAGS_control_command_topic;
  control_cmd_reader_config.pending_queue_size = FLAGS_control_cmd_pending_queue_size;
  if (FLAGS_receive_guardian) {
    guardian_cmd_reader_ = node_->CreateReader<GuardianCommand>(
        guardian_cmd_reader_config,[this](const std::shared_ptr<GuardianCommand> &cmd) {
          ADEBUG << "Received guardian data: run canbus callback.";
          OnGuardianCommand(*cmd);
        });
  } else {
    control_command_reader_ = node_->CreateReader<ControlCommand>(
        control_cmd_reader_config,[this](const std::shared_ptr<ControlCommand> &cmd) {
          ADEBUG << "Received control data: run canbus callback.";
          1.OnControlCommand(*cmd);
        });
  }
  //创建Chassis写入器
  chassis_writer_ = node_->CreateWriter<Chassis>(FLAGS_chassis_topic);
  //启动Canbus相关组件
  if (!vehicle_object_->Start()) {
    AERROR << "Fail to start canclient, cansender, canreceiver, canclient, "
              "vehicle controller.";//缺少判断？
  }
  AINFO << "Start canclient cansender, canreceiver, canclient, vehicle "
           "controller successfully.";
  monitor_logger_buffer_.INFO("Canbus is started.");
  return true;
}
bool CanbusComponent::Proc() {
  2.PublishChassis();
  if (FLAGS_enable_chassis_detail_pub) {
    vehicle_object_->PublishChassisDetail();
  }
  //表明正在运行
  vehicle_object_->UpdateHeartbeat();
  return true;
}
common::Status CanbusComponent::OnError(const std::string &error_msg) {
  monitor_logger_buffer_.ERROR(error_msg);
  return ::apollo::common::Status(ErrorCode::CANBUS_ERROR, error_msg); 
}

OnControlCommand(执行车辆控制命令)

void CanbusComponent::OnControlCommand(const ControlCommand &control_command) {
  int64_t current_timestamp = Time::Now().ToMicrosecond();
  //如果命令的到达速度超过了系统的处理能力，可以选择忽略
  if (current_timestamp - last_timestamp_ < FLAGS_min_cmd_interval * 1000) {
    ADEBUG << "Control command comes too soon. Ignore.\n Required "
              "FLAGS_min_cmd_interval["
           << FLAGS_min_cmd_interval << "], actual time interval[" << current_timestamp - last_timestamp_ << "].";
    return;
  }
  //记录新的时间戳
  last_timestamp_ = current_timestamp;
  //计算延迟
  ADEBUG << "Control_sequence_number:" << control_command.header().sequence_num() << ", Time_of_delay:"
         << current_timestamp - static_cast<int64_t>(control_command.header().timestamp_sec() *1e6) << " micro seconds";
  //将命令传递给底层的车辆控制系统
  vehicle_object_->UpdateCommand(&control_command);
}

PublishChassis(发布底盘信息)

void CanbusComponent::PublishChassis() {
  Chassis chassis = vehicle_object_->publish_chassis();
  //填充头部信息
  common::util::FillHeader(node_->Name(), &chassis);
  chassis_writer_->Write(chassis);
  ADEBUG << chassis.ShortDebugString();
}

Guardian模块

Guardian模块通过监测车辆的状态来识别潜在的风险，采取适当的措施来保障车辆运行的安全

输入：底盘(Chassis)，control_cmd以及system_status信息

输出：保护指令(guardian_cmd_)

关联模块：Canbus+Control

guardian_component.cc

bool GuardianComponent::Init() {
  if (!GetProtoConfig(&guardian_conf_)) {
    AERROR << "Unable to load canbus conf file: " << ConfigFilePath();
    return false;
  }
  //订阅chassis，control_cmd以及system_status信息
  chassis_reader_ = node_->CreateReader<Chassis>(FLAGS_chassis_topic, [this](const std::shared_ptr<Chassis>& chassis) {
        ADEBUG << "Received chassis data: run chassis callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        chassis_.CopyFrom(*chassis);
      });
  control_cmd_reader_ = node_->CreateReader<ControlCommand>(FLAGS_control_command_topic,[this](const std::shared_ptr<ControlCommand>& cmd) {
        ADEBUG << "Received control data: run control callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        control_cmd_.CopyFrom(*cmd);
      });
  system_status_reader_ = node_->CreateReader<SystemStatus>(FLAGS_system_status_topic,[this](const std::shared_ptr<SystemStatus>& status) {
        ADEBUG << "Received system status data: run system status callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        last_status_received_s_ = Time::Now().ToSecond();
        system_status_.CopyFrom(*status);
      });
  //guardian消息发布
  guardian_writer_ = node_->CreateWriter<GuardianCommand>(FLAGS_guardian_topic);
  return true;
}
bool GuardianComponent::Proc() {
  constexpr double kSecondsTillTimeout(2.5);
  bool safety_mode_triggered = false;
  //满足条件时进入安全模式
  if (guardian_conf_.guardian_enable()) {
    std::lock_guard<std::mutex> lock(mutex_);
    if (Time::Now().ToSecond() - last_status_received_s_ > kSecondsTillTimeout) {
      safety_mode_triggered = true;
    }
    safety_mode_triggered = safety_mode_triggered || system_status_.has_safety_mode_trigger_time();
  }
  //依据状态触发安全模式或继续传递控制命令
  if (safety_mode_triggered) {
    ADEBUG << "Safety mode triggered, enable safety mode";
    1.TriggerSafetyMode();
  } else {
    ADEBUG << "Safety mode not triggered, bypass control command";
    PassThroughControlCommand();
  }
  //填充头部信息并发布命令
  common::util::FillHeader(node_->Name(), &guardian_cmd_);
  guardian_writer_->Write(guardian_cmd_);
  return true;
}

TriggerSafetyMode(触发安全模式)

void GuardianComponent::TriggerSafetyMode() {
  AINFO << "Safety state triggered, with system safety mode trigger time : " << system_status_.safety_mode_trigger_time();
  std::lock_guard<std::mutex> lock(mutex_);
  bool sensor_malfunction = false, obstacle_detected = false;
  //检查超声波传感器
  if (!chassis_.surround().sonar_enabled() || chassis_.surround().sonar_fault()) {
    AINFO << "Ultrasonic sensor not enabled for faulted, will do emergency stop!";
    sensor_malfunction = true;
  } else {
    //当障碍物过近或传感器故障
    for (int i = 0; i < chassis_.surround().sonar_range_size(); ++i) {
      if ((chassis_.surround().sonar_range(i) > 0.0 &&
           chassis_.surround().sonar_range(i) < 2.5) ||
          chassis_.surround().sonar_range(i) > 30) {
        AINFO << "Object detected or ultrasonic sensor fault output, will do emergency stop!";
        obstacle_detected = true;
      }
    }
  }
  //设置安全模式下控制命令
  guardian_cmd_.mutable_control_command()->set_throttle(0.0);
  guardian_cmd_.mutable_control_command()->set_steering_target(0.0);
  guardian_cmd_.mutable_control_command()->set_steering_rate(25.0);
  guardian_cmd_.mutable_control_command()->set_is_in_safe_mode(true);
  //***TODO(QiL) : Remove this one once hardware re-alignment is done.(重新校准)
  sensor_malfunction = false;
  obstacle_detected = false;
  AINFO << "Temporarily ignore the ultrasonic sensor output during hardware re-alignment!";
  //根据情况采取制动
  if (system_status_.require_emergency_stop() || sensor_malfunction || obstacle_detected) {
    AINFO << "Emergency stop triggered! with system status from monitor as : "
          << system_status_.require_emergency_stop();
    guardian_cmd_.mutable_control_command()->set_brake(guardian_conf_.guardian_cmd_emergency_stop_percentage());
  } else {
    AINFO << "Soft stop triggered! with system status from monitor as : "
          << system_status_.require_emergency_stop();
    guardian_cmd_.mutable_control_command()->set_brake(guardian_conf_.guardian_cmd_soft_stop_percentage());
  }
}