Apollo源码

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

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

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;
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

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);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

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;
}
1
2
3
4
5
6
7
8
9
10

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();
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

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);
}
1
2
3
4
5
6
7
8
9
10
11

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

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());
    }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67

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;
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147

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 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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

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;
    }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

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

Routing模块

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

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

输出：规划的路径

关联模块：Map

routing_component.cc

#include "modules/routing/routing_component.h"
#include 
#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;
}
}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72

routing.cc

#include "modules/routing/routing.h"
#include 
#include 
#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;
}
}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

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); 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

PublishChassis(发布底盘信息)

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

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

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());
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39