Apollo9.0 PNC源码学习之Control模块（一）—— 控制模块概览_apollo control模块

0 前言

从planning的角度看control，首先需要了解的就是相关的数据接口，规划出的轨迹（路径+速度）发给Control模块去执行
modules/planning/planning_component/planning_component.cc
planning模块发布轨迹信息

planning_writer_ = node_->CreateWriter<ADCTrajectory>(
      config_.topic_config().planning_trajectory_topic());
1
2

modules/control/control_component/control_component.cc
Control模块接受轨迹信息

trajectory_reader_ =
      node_->CreateReader<ADCTrajectory>(planning_reader_config, nullptr);
ACHECK(trajectory_reader_ != nullptr);
1
2
3

ADCTrajectory在modules/common_msgs/planning_msgs/planning.proto定义

message ADCTrajectory {
  optional apollo.common.Header header = 1;

  optional double total_path_length = 2;  // in meters

  optional double total_path_time = 3;    // in seconds

  optional EStop estop = 6;

  optional apollo.planning_internal.Debug debug = 8;

  // is_replan == true mean replan triggered 重规划 
  optional bool is_replan = 9 [default = false];

  // Specify trajectory gear 档位
  optional apollo.canbus.Chassis.GearPosition gear = 10;

  // path data + speed data 路径数据 + 速度数据
  repeated apollo.common.TrajectoryPoint trajectory_point = 12;

  // path point without speed info 路径点
  repeated apollo.common.PathPoint path_point = 13;

  optional apollo.planning.DecisionResult decision = 14;

  optional LatencyStats latency_stats = 15;

  // the routing used for current planning result
  optional apollo.common.Header routing_header = 16;
  enum RightOfWayStatus {
    UNPROTECTED = 0;
    PROTECTED = 1;
  }
  optional RightOfWayStatus right_of_way_status = 17;

  // lane id along current reference line
  repeated apollo.hdmap.Id lane_id = 18;

  // set the engage advice for based on current planning result.
  optional apollo.common.EngageAdvice engage_advice = 19;

  // the region where planning cares most
  message CriticalRegion {
    repeated apollo.common.Polygon region = 1;
  }
  // critical region will be empty when planning is NOT sure which region is
  // critical
  // critical regions may or may not overlap
  optional CriticalRegion critical_region = 20;
  // 轨迹类型（未知、正常、）
  enum TrajectoryType {
    UNKNOWN = 0;       // 未知的轨迹类型，通常用于表示无法确定或识别的情况
    NORMAL = 1;        // 正常的轨迹类型，可能是由标准路径规划算法生成的轨迹
    PATH_FALLBACK = 2; // 路径回退类型，当标准路径规划失败时，可能会使用备用路径规划算法生成轨迹
    SPEED_FALLBACK = 3;// 速度回退类型，当无法满足速度约束条件时，可能会使用备用速度规划算法生成轨迹
    PATH_REUSED = 4;   // 重用路径类型，可能是之前生成的路径的重用或修改版本
    OPEN_SPACE = 5;    // 开放空间类型，通常用于表示在开放环境中的轨迹规划，比如停车或避障等情况
  }
  optional TrajectoryType trajectory_type = 21 [default = UNKNOWN];

  optional string replan_reason = 22;

  // lane id along target reference line
  repeated apollo.hdmap.Id target_lane_id = 23;

  // complete dead end flag
  optional bool car_in_dead_end = 24;

  // output related to RSS
  optional RSSInfo rss_info = 100;
}
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

1 纵览控制模块

Control模块由control组件包和controller控制器组成，control组件包包含control的整体架构和流程。control根据上游模块输入planning模块的期望轨迹信息，定位模块的当前定位信息，车辆底盘及车身状态信息，通过不同的控制算法计算控制车辆的指令（包含转向、油门、刹车等）输出给canbus模块

1.1 control_component

control_component是继承于apollo::cyber::TimerComponent的子类，是一个定时触发的组件，通过dag配置可以修改定时器周期。Init和Proc是入口函数，在初始化函数中，主要实现了ControlTaskAgent的初始化，以及control上游的相关消息的订阅。在Proc执行函数中，分别执行了几步操作：获取订阅消息的当前最新数据–>检查订阅消息输入数据（代码里主要检查了对轨迹线数据是否为空的检查，其它消息数据的检查也可以自行添加）是否完整–>检查订阅消息输入数据时间戳是否在容差范围内（上游消息的数据周期是否超时，如果超时control会有紧急处理）–>更新车身姿态信息–>进行control控制计算（这部分调用ControlTaskAgent的ComputeControlCommand方法，ControlTaskAgent通过配置文件，管理控制器ControlTask的加载和执行顺序，进而完成控制指令的计算）–>输出底盘控制指令

1.2 control_task_base

control_component/controller_task_base/主要包含ControlTaskAgent和ControlTask定义。ControlTaskAgent用来管理ControlTask插件的加载和执行顺序，ControlTask是controller控制器插件的父类，Control/controller控制器插件都继承于ControlTask，目前Apollo已经支持的控制器插件有横向控制器（LatController），纵向控制器（LonController），MPC控制器（MPCController），以及DemoControlTask任务器（DemoControlTask）

1.3 controller

Apollo对车辆的控制是将车辆在车体坐标系转换到Frenet坐标系下进行位置跟踪，将车辆跟踪轨迹的运动分解为横向运动和纵向运动，通过对车体的动力学建模，选取合适的状态变量对车辆的跟踪情况进行观测，再通过横向和纵向的控制算法，计算合理的控制指令，达到对轨迹线的跟踪目标

1.4 文件组织结构及说明

control/
├── control_component/                  // control基础组件
    ├── common                          // 模块全局gflag定义
    ├── conf                            // 模块配置文件，参数文件目录，包含gflags变量的配置，插件启用的配置文件，车辆标定表等通用的配置文件
    ├── controller_task_base/           // control控制器父类组件
    │   ├── common/                     // 数学公式，算法公式，滤波函数，轨迹分析
    │   ├── integration_tests/          // 单元测试文件夹
    │   ├── control_task_agent.cc       // 控制器加载管理器实现文件
    │   ├── control_task_agent.h        // 控制器加载管理器实现文件
    │   └── control_task.h              // 控制器父类实现文件
    ├── dag/                            // 模块启动文件(mainboard)
    ├── docs/                           // 相关模块说明文档
    ├── launch/                         // 模块启动文件(cyber_launch)
    ├── proto/                          // 组件定义的配置文件
    ├── submodules/                     // control子模块
    ├── testdata/                       // 单元测试数据
    ├── tools/                          // 调试工具
    ├── BUILD                           // 构建规则文件
    ├── control_component.cc            // 组件实现的代码文件
    ├── control_component.h             // 组件实现的代码文件
    ├── control_component_test.cc       // 组件单元测试文件
    ├── control.json                    // 打包描述文件
    ├── cyberfile.xml                   // 包管理配置文件
    └── README_cn.md                    // 说明文档
└── controllers/                        // 控制器算法或逻辑任务组件
    ├── demo_control_task               // demo控制器插件包
    │   ├── proto/                      // 控制器的配置定义文件夹
    │   ├── conf/                       // 控制器配置文件夹
    │   ├── BUILD                       // 构建规则文件
    │   ├── cyberfile.xml               // 包管理配置文件
    │   ├── demo_control_task.cc        // demo控制器实现文件
    │   ├── demo_control_task.h         // demo控制器实现文件
    │   └── plugins.xml                 // 插件规则文件
    ├── lat_based_lqr_controller        // LQR横向控制器插件包
    │   ├── proto/                      // 控制器的配置定义文件夹
    │   ├── conf/                       // 控制器配置文件夹
    │   ├── BUILD                       // 构建规则文件
    │   ├── cyberfile.xml               // 包管理配置文件
    │   ├── lat_controller.cc           // LQR横向控制器实现文件
    │   ├── lat_controller.h            // LQR横向控制器实现文件
    │   ├── lat_controller_test.cc      // LQR横向控制器单元测试文件
    │   ├── lateral_controller_test     // 控制器测试数据
    │   └── plugins.xml                 // 插件规则文件
    ├── lon_based_pid_controller        // PID纵向控制器插件包
    │   ├── proto/                      // 控制器的配置定义文件夹
    │   ├── conf/                       // 控制器配置文件夹
    │   ├── BUILD                       // 构建规则文件
    │   ├── cyberfile.xml               // 包管理配置文件
    │   ├── lon_controller.cc           // PID纵向控制器实现文件
    │   ├── lon_controller.h            // PID纵向控制器实现文件
    │   ├── lon_controller_test.cc      // PID纵向控制器单元测试文件
    │   ├── longitudinal_controller_test// 控制器测试数据
    │   └── plugins.xml                 // 插件规则文件
    └── mpc_controller                  // MPC横纵向控制器插件包
        ├── proto/                      // 控制器的配置定义文件夹
        ├── conf/                       // 控制器配置文件夹
        ├── BUILD                       // 构建规则文件
        ├── cyberfile.xml               // 包管理配置文件
        ├── mpc_controller.cc           // MPC控制器实现文件
        ├── mpc_controller.h            // MPC控制器实现文件
        ├── mpc_controller_test.cc      // MPC控制器单元测试文件
        ├── mpc_controller_test_data    // 控制器测试数据
        └── plugins.xml                 // 插件规则文件
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

1.5 模块输入输出与配置

输入：

输出：

配置文件：

Flags：

2 控制器组件代码解析

control_component.h

#pragma once

#include <memory>
#include <string>

#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
#include "modules/common_msgs/control_msgs/pad_msg.pb.h"
#include "modules/common_msgs/external_command_msgs/command_status.pb.h"
#include "modules/common_msgs/localization_msgs/localization.pb.h"
#include "modules/common_msgs/planning_msgs/planning.pb.h"
#include "modules/control/control_component/proto/preprocessor.pb.h"

#include "cyber/class_loader/class_loader.h"
#include "cyber/component/timer_component.h"
#include "cyber/time/time.h"
#include "modules/common/monitor_log/monitor_log_buffer.h"
#include "modules/common/util/util.h"
#include "modules/control/control_component/controller_task_base/common/dependency_injector.h"
#include "modules/control/control_component/controller_task_base/control_task_agent.h"
#include "modules/control/control_component/submodules/preprocessor_submodule.h"

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

/**
 * @class Control
 *
 * @brief control module main class, it processes localization, chassis, and
 * pad data to compute throttle, brake and steer values.
 */
// 控制模块主类，处理定位、底盘、pad数据为了计算油门、刹车和转向，继承apollo::cyber::TimerComponent，定时触发
class ControlComponent final : public apollo::cyber::TimerComponent {
  friend class ControlTestBase;

 public:
  ControlComponent();
  bool Init() override;

  bool Proc() override;

 private:
  // Upon receiving pad message
  // 接收pad消息
  void OnPad(const std::shared_ptr<PadMessage> &pad);
  // 接收底盘消息
  void OnChassis(const std::shared_ptr<apollo::canbus::Chassis> &chassis);
  // 接受轨迹消息
  void OnPlanning(
      const std::shared_ptr<apollo::planning::ADCTrajectory> &trajectory);
  // 规划命令状态信息
  void OnPlanningCommandStatus(
      const std::shared_ptr<external_command::CommandStatus>
          &planning_command_status);
  // 接收定位消息
  void OnLocalization(
      const std::shared_ptr<apollo::localization::LocalizationEstimate>
          &localization);

  // Upon receiving monitor message
  // 接收检测信息
  void OnMonitor(
      const apollo::common::monitor::MonitorMessage &monitor_message);

  common::Status ProduceControlCommand(ControlCommand *control_command);
  common::Status CheckInput(LocalView *local_view);
  common::Status CheckTimestamp(const LocalView &local_view);
  common::Status CheckPad();
  void ResetAndProduceZeroControlCommand(ControlCommand *control_command);
  void GetVehiclePitchAngle(ControlCommand *control_command);

 private:
  apollo::cyber::Time init_time_;

  localization::LocalizationEstimate latest_localization_;
  canbus::Chassis latest_chassis_;
  planning::ADCTrajectory latest_trajectory_;
  external_command::CommandStatus planning_command_status_;
  PadMessage pad_msg_;
  common::Header latest_replan_trajectory_header_;

  ControlTaskAgent control_task_agent_;

  bool estop_ = false;
  std::string estop_reason_;
  bool pad_received_ = false;

  unsigned int status_lost_ = 0;
  unsigned int status_sanity_check_failed_ = 0;
  unsigned int total_status_lost_ = 0;
  unsigned int total_status_sanity_check_failed_ = 0;

  ControlPipeline control_pipeline_;

  std::mutex mutex_;
  // 订阅者 底盘、pad、定位、轨迹、命令状态
  std::shared_ptr<cyber::Reader<apollo::canbus::Chassis>> chassis_reader_;
  std::shared_ptr<cyber::Reader<PadMessage>> pad_msg_reader_;
  std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
      localization_reader_;
  std::shared_ptr<cyber::Reader<apollo::planning::ADCTrajectory>>
      trajectory_reader_;
  std::shared_ptr<cyber::Reader<apollo::external_command::CommandStatus>>
      planning_command_status_reader_;
  // 发布者 控制命令、使用控制子模块LocalView
  std::shared_ptr<cyber::Writer<ControlCommand>> control_cmd_writer_;
  // when using control submodules
  std::shared_ptr<cyber::Writer<LocalView>> local_view_writer_;
  
  common::monitor::MonitorLogBuffer monitor_logger_buffer_;

  LocalView local_view_;

  std::shared_ptr<DependencyInjector> injector_;

  double previous_steering_command_ = 0.0;
};

CYBER_REGISTER_COMPONENT(ControlComponent)
}  // namespace control
}  // namespace apollo
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

control_component.cc

#include "modules/control/control_component/control_component.h"

#include "absl/strings/str_cat.h"

#include "cyber/common/file.h"
#include "cyber/common/log.h"
#include "cyber/time/clock.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/latency_recorder/latency_recorder.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/control/control_component/common/control_gflags.h"

namespace apollo {
namespace control {

using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::VehicleStateProvider;
using apollo::cyber::Clock;
using apollo::localization::LocalizationEstimate;
using apollo::planning::ADCTrajectory;

const double kDoubleEpsilon = 1e-6;

ControlComponent::ControlComponent()
    : monitor_logger_buffer_(common::monitor::MonitorMessageItem::CONTROL) {}

bool ControlComponent::Init() {
  injector_ = std::make_shared<DependencyInjector>();
  init_time_ = Clock::Now();

  AINFO << "Control init, starting ...";

  ACHECK(
      cyber::common::GetProtoFromFile(FLAGS_pipeline_file, &control_pipeline_))
      << "Unable to load control pipeline file: " + FLAGS_pipeline_file;

  AINFO << "ControlTask pipeline config file: " << FLAGS_pipeline_file
        << " is loaded.";

  // initial controller agent when not using control submodules
  ADEBUG << "FLAGS_use_control_submodules: " << FLAGS_use_control_submodules;
  if (!FLAGS_is_control_ut_test_mode) {
    if (!FLAGS_use_control_submodules &&
        !control_task_agent_.Init(injector_, control_pipeline_).ok()) {
      // set controller
      ADEBUG << "original control";
      monitor_logger_buffer_.ERROR(
          "Control init controller failed! Stopping...");
      return false;
    }
  }

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

  cyber::ReaderConfig planning_command_status_reader_config;
  planning_command_status_reader_config.channel_name =
      FLAGS_planning_command_status;
  planning_command_status_reader_config.pending_queue_size =
      FLAGS_planning_status_msg_pending_queue_size;
  // 订阅规划命令状态
  planning_command_status_reader_ =
      node_->CreateReader<external_command::CommandStatus>(
          planning_command_status_reader_config, nullptr);
  ACHECK(planning_command_status_reader_ != nullptr);

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

  cyber::ReaderConfig pad_msg_reader_config;
  pad_msg_reader_config.channel_name = FLAGS_pad_topic;
  pad_msg_reader_config.pending_queue_size = FLAGS_pad_msg_pending_queue_size;
  // 订阅pad消息
  pad_msg_reader_ =
      node_->CreateReader<PadMessage>(pad_msg_reader_config, nullptr);
  ACHECK(pad_msg_reader_ != nullptr);
  // 如果使用控制子模块，发布控制话题，否则，发布控制local_view
  if (!FLAGS_use_control_submodules) {
    control_cmd_writer_ =
        node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
    ACHECK(control_cmd_writer_ != nullptr);
  } else {
    local_view_writer_ =
        node_->CreateWriter<LocalView>(FLAGS_control_local_view_topic);
    ACHECK(local_view_writer_ != nullptr);
  }

  // set initial vehicle state by cmd
  // need to sleep, because advertised channel is not ready immediately
  // simple test shows a short delay of 80 ms or so
  // 休眠1000ms
  AINFO << "Control resetting vehicle state, sleeping for 1000 ms ...";
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // should init_vehicle first, let car enter work status, then use status msg
  // trigger control
  // 首先初始化vehicle，让车到工作状态，然后使用状态消息
  AINFO << "Control default driving action is "
        << DrivingAction_Name((enum DrivingAction)FLAGS_action);
  pad_msg_.set_action((enum DrivingAction)FLAGS_action);

  return true;
}

void ControlComponent::OnPad(const std::shared_ptr<PadMessage> &pad) {
  // 创建锁，确保在访问 pad_msg_ 期间不会发生数据竞争
  std::lock_guard<std::mutex> lock(mutex_);
  // 将 pad 中的数据复制到 pad_msg_
  pad_msg_.CopyFrom(*pad);
  ADEBUG << "Received Pad Msg:" << pad_msg_.DebugString();
  AERROR_IF(!pad_msg_.has_action()) << "pad message check failed!";
}

void ControlComponent::OnChassis(const std::shared_ptr<Chassis> &chassis) {
  ADEBUG << "Received chassis data: run chassis callback.";
  // 创建锁，确保在访问 latest_chassis_ 期间不会发生数据竞争
  std::lock_guard<std::mutex> lock(mutex_);
  latest_chassis_.CopyFrom(*chassis);
}

void ControlComponent::OnPlanning(
    const std::shared_ptr<ADCTrajectory> &trajectory) {
  ADEBUG << "Received chassis data: run trajectory callback.";
  // 创建锁，确保在访问 latest_trajectory_ 期间不会发生数据竞争
  std::lock_guard<std::mutex> lock(mutex_);
  latest_trajectory_.CopyFrom(*trajectory);
}

void ControlComponent::OnPlanningCommandStatus(
    const std::shared_ptr<external_command::CommandStatus>
        &planning_command_status) {
  ADEBUG << "Received plannning command status data: run planning command "
            "status callback.";
  // 创建锁，确保在访问 planning_command_status_ 期间不会发生数据竞争
  std::lock_guard<std::mutex> lock(mutex_);
  planning_command_status_.CopyFrom(*planning_command_status);
}

void ControlComponent::OnLocalization(
    const std::shared_ptr<LocalizationEstimate> &localization) {
  ADEBUG << "Received control data: run localization message callback.";
  // 创建锁，确保在访问 latest_localization_ 期间不会发生数据竞争
  std::lock_guard<std::mutex> lock(mutex_);
  latest_localization_.CopyFrom(*localization);
}

void ControlComponent::OnMonitor(
    const common::monitor::MonitorMessage &monitor_message) {
  for (const auto &item : monitor_message.item()) {
    if (item.log_level() == common::monitor::MonitorMessageItem::FATAL) {
      // 检测到严重问题，需要立即停止
      estop_ = true;
      return;
    }
  }
}

Status ControlComponent::ProduceControlCommand(
    ControlCommand *control_command) {
  // 检查输入数据
  Status status = CheckInput(&local_view_);
  // check data
  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 {
    estop_ = false;
    // 检查时间戳
    Status status_ts = CheckTimestamp(local_view_);
    if (!status_ts.ok()) {
      AERROR << "Input messages timeout";
      // Clear trajectory data to make control stop if no data received again
      // next cycle.
      // keep the history trajectory for control compute.
      // latest_trajectory_.Clear();
      estop_ = true;
      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_ = false;
    }
  }

  // 检查 estop
  estop_ = FLAGS_enable_persistent_estop
               ? estop_ || local_view_.trajectory().estop().is_estop()
               : local_view_.trajectory().estop().is_estop();
  // 如果规划中的 estop 标志为真，则设置 estop_ 为真
  if (local_view_.trajectory().estop().is_estop()) {
    estop_ = true;
    estop_reason_ = "estop from planning : ";
    estop_reason_ += local_view_.trajectory().estop().reason();
  }
  // 如果规划中的轨迹点为空，则设置 estop_ 为真
  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();
  }
  // 如果启用了 gear_drive 负速度保护功能，并且当前驾驶模式为 gear_drive，并且第一个轨迹点的速度小于 -kEpsilon，则设置 estop_ 为真
  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) {
      control_task_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_);
    }
  }
  // 如果当前规划的轨迹点不为空，则调用控制任务代理计算控制命令
  if (!local_view_.trajectory().trajectory_point().empty()) {
    // controller agent
    Status status_compute = control_task_agent_.ComputeControlCommand(
        &local_view_.localization(), &local_view_.chassis(),
        &local_view_.trajectory(), control_command);
    ADEBUG << "status_compute is " << status_compute;
    // 如果计算控制命令失败，记录错误信息并设置 estop_ 为 true
    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;
    }
  }

  // if planning set estop, then no control process triggered
  // 如果规划停止，控制就触发不了
  if (estop_) {
    AWARN_EVERY(100) << "Estop triggered! No control core method executed!";
    // set Estop command
    control_command->set_speed(0);
    control_command->set_throttle(0);
    control_command->set_brake(FLAGS_soft_estop_brake);
    control_command->set_gear_location(Chassis::GEAR_DRIVE);
    previous_steering_command_ =
        injector_->previous_control_command_mutable()->steering_target();
    control_command->set_steering_target(previous_steering_command_);
  }
  // check signal
  if (local_view_.trajectory().decision().has_vehicle_signal()) {
    control_command->mutable_signal()->CopyFrom(
        local_view_.trajectory().decision().vehicle_signal());
  }
  return status;
}
// 核心函数Proc
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!";
    injector_->set_control_process(false);
    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!";
  } else {
    // Check if new planning data received.
    if (latest_trajectory_.header().sequence_num() !=
        trajectory_msg->header().sequence_num()) {
      OnPlanning(trajectory_msg);
    }
  }

  planning_command_status_reader_->Observe();
  const auto &planning_status_msg =
      planning_command_status_reader_->GetLatestObserved();
  if (planning_status_msg != nullptr) {
    OnPlanningCommandStatus(planning_status_msg);
    ADEBUG << "Planning command status msg is \n"
           << planning_command_status_.ShortDebugString();
  }
  injector_->set_planning_command_status(planning_command_status_);

  localization_reader_->Observe();
  const auto &localization_msg = localization_reader_->GetLatestObserved();
  // 接收不到定位消息
  if (localization_msg == nullptr) {
    AERROR << "localization msg is not ready!";
    injector_->set_control_process(false);
    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): to avoid redundent copy
    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_);
    }
  }

  // use control submodules
  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();

    // measure latency
    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;
  }

  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 (FLAGS_is_control_test_mode && FLAGS_control_test_duration > 0 &&
      (start_time - init_time_).ToSecond() > FLAGS_control_test_duration) {
    AERROR << "Control finished testing. exit";
    injector_->set_control_process(false);
    return false;
  }

  injector_->set_control_process(true);

  ControlCommand control_command;

  Status status;
  // 自动驾驶模式
  if (local_view_.chassis().driving_mode() ==
      apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    status = ProduceControlCommand(&control_command);
    ADEBUG << "Produce control command normal.";
  } else {
    ADEBUG << "Into reset control command.";
    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;
  }

  // forward estop reason among following control frames.
  if (estop_) {
    control_command.mutable_header()->mutable_status()->set_msg(estop_reason_);
  }

  // set header
  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 (FLAGS_is_control_test_mode) {
    ADEBUG << "Skip publish control command in test mode";
    return true;
  }

  if (fabs(control_command.debug().simple_lon_debug().vehicle_pitch()) <
      kDoubleEpsilon) {
    injector_->vehicle_state()->Update(local_view_.localization(),
                                       local_view_.chassis());
    GetVehiclePitchAngle(&control_command);
  }

  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 > FLAGS_control_period * 1e3);
  ADEBUG << "control cycle time is: " << time_diff_ms << " ms.";
  status.Save(control_command.mutable_header()->mutable_status());

  // measure latency
  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);
  }

  // save current control command 保存当前控制命令
  injector_->Set_pervious_control_command(&control_command);
  injector_->previous_control_command_mutable()->CopyFrom(control_command);
  injector_->previous_control_debug_mutable()->CopyFrom(
      injector_->control_debug_info());
  // 发布控制命令
  control_cmd_writer_->Write(control_command);
  return true;
}
// 检查输入
Status ControlComponent::CheckInput(LocalView *local_view) {
  ADEBUG << "Received localization:"
         << local_view->localization().ShortDebugString();
  ADEBUG << "Received chassis:" << local_view->chassis().ShortDebugString();

  if (!local_view->trajectory().estop().is_estop() &&
      local_view->trajectory().trajectory_point().empty()) {
    AWARN_EVERY(100) << "planning has no trajectory point. ";
    const std::string msg =
        absl::StrCat("planning has no trajectory point. planning_seq_num:",
                     local_view->trajectory().header().sequence_num());
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, msg);
  }

  for (auto &trajectory_point :
       *local_view->mutable_trajectory()->mutable_trajectory_point()) {
    if (std::abs(trajectory_point.v()) < FLAGS_minimum_speed_resolution &&
        std::abs(trajectory_point.a()) < FLAGS_max_acceleration_when_stopped) {
      trajectory_point.set_v(0.0);
      trajectory_point.set_a(0.0);
    }
  }

  injector_->vehicle_state()->Update(local_view->localization(),
                                     local_view->chassis());

  return Status::OK();
}
// 检查时间戳
Status ControlComponent::CheckTimestamp(const LocalView &local_view) {
  if (!FLAGS_enable_input_timestamp_check || FLAGS_is_control_test_mode) {
    ADEBUG << "Skip input timestamp check by gflags.";
    return Status::OK();
  }
  double current_timestamp = Clock::NowInSeconds();
  double localization_diff =
      current_timestamp - local_view.localization().header().timestamp_sec();
  if (localization_diff >
      (FLAGS_max_localization_miss_num * FLAGS_localization_period)) {
    AERROR << "Localization msg lost for " << std::setprecision(6)
           << localization_diff << "s";
    monitor_logger_buffer_.ERROR("Localization msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Localization msg timeout");
  }

  double chassis_diff =
      current_timestamp - local_view.chassis().header().timestamp_sec();
  if (chassis_diff > (FLAGS_max_chassis_miss_num * FLAGS_chassis_period)) {
    AERROR << "Chassis msg lost for " << std::setprecision(6) << chassis_diff
           << "s";
    monitor_logger_buffer_.ERROR("Chassis msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Chassis msg timeout");
  }

  double trajectory_diff =
      current_timestamp - local_view.trajectory().header().timestamp_sec();
  if (trajectory_diff >
      (FLAGS_max_planning_miss_num * FLAGS_trajectory_period)) {
    AERROR << "Trajectory msg lost for " << std::setprecision(6)
           << trajectory_diff << "s";
    monitor_logger_buffer_.ERROR("Trajectory msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Trajectory msg timeout");
  }
  return Status::OK();
}
// 重置控制命令
void ControlComponent::ResetAndProduceZeroControlCommand(
    ControlCommand *control_command) {
  control_command->set_throttle(0.0);
  control_command->set_steering_target(0.0);
  control_command->set_steering_rate(0.0);
  control_command->set_speed(0.0);
  control_command->set_brake(0.0);
  control_command->set_gear_location(Chassis::GEAR_DRIVE);
  control_task_agent_.Reset();
  latest_trajectory_.mutable_trajectory_point()->Clear();
  latest_trajectory_.mutable_path_point()->Clear();
  trajectory_reader_->ClearData();
}
// 获得汽车的俯仰角
void ControlComponent::GetVehiclePitchAngle(ControlCommand *control_command) {
  double vehicle_pitch = injector_->vehicle_state()->pitch() * 180 / M_PI;
  control_command->mutable_debug()
      ->mutable_simple_lon_debug()
      ->set_vehicle_pitch(vehicle_pitch + FLAGS_pitch_offset_deg);
}

}  // namespace control
}  // namespace apollo
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
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585

控制全局变量配置文件见control_gflags.cc

配置加载的控制器，Apollo中modules/control/control_component/conf/pipeline.pb.txt

controller {
  name: "LAT_CONTROLLER"
  type: "LatController"
}
controller {
  name: "LON_CONTROLLER"
  type: "LonController"
}
1
2
3
4
5
6
7
8

name是用户自定义，表达清楚是什么控制器就行，type是控制器的子类名称，如果和子类名称不一致，会导致加载控制器失败。上面是先加载横向控制器，再加载纵向控制器

3 Control组件包逻辑梳理

主要是梳理Init函数和Proc函数
Init函数：
主要实现ControlTaskAgent的初始化，以及control上游的相关消息的订阅

摘取Init函数里面的主要部分

bool ControlComponent::Init() {
  // 初始化控制器agent
  if (!FLAGS_is_control_ut_test_mode) {
    if (!FLAGS_use_control_submodules &&
        !control_task_agent_.Init(injector_, control_pipeline_).ok()) {
      // set controller
      ADEBUG << "original control";
      monitor_logger_buffer_.ERROR(
          "Control init controller failed! Stopping...");
      return false;
    }
  }
  // 订阅底盘信息
  chassis_reader_ =
      node_->CreateReader<Chassis>(chassis_reader_config, nullptr);
  // 订阅轨迹信息
  trajectory_reader_ =
      node_->CreateReader<ADCTrajectory>(planning_reader_config, nullptr);
  // 订阅规划命令状态
  planning_command_status_reader_ =
      node_->CreateReader<external_command::CommandStatus>(
          planning_command_status_reader_config, nullptr);
  // 订阅定位信息
  localization_reader_ = node_->CreateReader<LocalizationEstimate>(
      localization_reader_config, nullptr);
  // 订阅pad消息
  pad_msg_reader_ =
      node_->CreateReader<PadMessage>(pad_msg_reader_config, nullptr);
}
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

Proc函数：
(1) 获取订阅消息的当前最新数据
(2) 检查订阅消息输入数据
(3) 检查订阅消息输入数据时间戳是否在容差范围内
(4) 更新车身姿态信息
(5) 进行control控制计算(调用ControlTaskAgent的ComputeControlCommand方法)
(6) 输出底盘控制指令

摘取Proc函数里面的主要部分

bool ControlComponent::Proc() {
    // 获取订阅消息的当前最新数据
	const auto &chassis_msg = chassis_reader_->GetLatestObserved();
	const auto &trajectory_msg = trajectory_reader_->GetLatestObserved();
	const auto &planning_status_msg =
      planning_command_status_reader_->GetLatestObserved();
    const auto &localization_msg = localization_reader_->GetLatestObserved();
    const auto &pad_msg = pad_msg_reader_->GetLatestObserved();
    // 检查订阅消息输入数据
    // 检查订阅消息输入数据时间戳是否在容差范围内
    // 自动驾驶模式
    if (local_view_.chassis().driving_mode() ==
        apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    // 计算控制命令
      status = ProduceControlCommand(&control_command);
      ADEBUG << "Produce control command normal.";
    } else {
      ADEBUG << "Into reset control command.";
      ResetAndProduceZeroControlCommand(&control_command);
    }
    // 更新车身姿态信息
    injector_->vehicle_state()->Update(local_view_.localization(),
                                       local_view_.chassis());
    // 发布控制命令
    control_cmd_writer_->Write(control_command);
}
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

3 controller_task_base

之前的控制组件包讲解告一段落，如有疑惑可在评论区留言讨论

controller_task_base主要包含ControlTaskAgent和ControlTask定义，ControlTaskAgent用来管理ControlTask插件的加载和执行顺序，ControlTask是controller控制器插件的父类，Control/controller控制器插件都继承于ControlTask

control_task_agent.h：

#pragma once

#include <memory>
#include <vector>

#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
#include "modules/common_msgs/planning_msgs/planning.pb.h"
#include "modules/control/control_component/proto/pipeline.pb.h"

#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/common/util/factory.h"
#include "modules/control/control_component/controller_task_base/common/dependency_injector.h"
#include "modules/control/control_component/controller_task_base/control_task.h"

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

/**
 * @class ControlTaskAgent
 *
 * @brief manage all controllers declared in control config file.
 */
class ControlTaskAgent {
 public:
  /**
   * @brief 初始化 ControlTaskAgent
   * @param control_conf control configurations
   * @return Status initialization status
   */
  common::Status Init(std::shared_ptr<DependencyInjector> injector,
                      const ControlPipeline &control_pipeline);

  /**
   * @brief compute control command based on current vehicle status
   *        and target trajectory
   * @param localization vehicle location
   * @param chassis vehicle status e.g., speed, acceleration
   * @param trajectory trajectory generated by planning
   * @param cmd control command
   * @return Status computation status
   */
  // 基于当前车辆状态和目标轨迹计算控制命令
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd);

  /**
   * @brief reset ControlTaskAgent
   * @return Status reset status
   */
  // 重置ControlTaskAgent
  common::Status Reset();

 private:
  std::vector<std::shared_ptr<ControlTask>> controller_list_;
  std::shared_ptr<DependencyInjector> injector_ = nullptr;
};

}  // namespace control
}  // namespace apollo
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

control_task_agent.cc

#include "modules/control/control_component/controller_task_base/control_task_agent.h"

#include <utility>

#include "cyber/common/log.h"
#include "cyber/time/clock.h"
#include "modules/control/control_component/common/control_gflags.h"

namespace apollo {
namespace control {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::cyber::Clock;
using apollo::cyber::plugin_manager::PluginManager;
// 初始化控制器
Status ControlTaskAgent::Init(std::shared_ptr<DependencyInjector> injector,
                              const ControlPipeline &control_pipeline) {
  if (control_pipeline.controller_size() == 0) {
    AERROR << "control_pipeline is empty";
    return Status(ErrorCode::CONTROL_INIT_ERROR, "Empty control_pipeline");
  }

  injector_ = injector;
  for (int i = 0; i < control_pipeline.controller_size(); i++) {
    auto controller = PluginManager::Instance()->CreateInstance<ControlTask>(
        "apollo::control::" + control_pipeline.controller(i).type());
    if (!controller->Init(injector_).ok()) {
      AERROR << "Can not init controller " << controller->Name();
      return Status(
          ErrorCode::CONTROL_INIT_ERROR,
          "Failed to init Controller:" + control_pipeline.controller(i).name());
    }
    controller_list_.push_back(controller);
    AINFO << "Controller <" << controller->Name() << "> init done!";
  }
  return Status::OK();
}
// 计算控制命令
Status ControlTaskAgent::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
    control::ControlCommand *cmd) {
  for (auto &controller : controller_list_) {
    ADEBUG << "controller:" << controller->Name() << " processing ...";
    double start_timestamp = Clock::NowInSeconds();
    // 计算控制命令 （核心）
    controller->ComputeControlCommand(localization, chassis, trajectory, cmd);
    double end_timestamp = Clock::NowInSeconds();
    const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;

    ADEBUG << "controller: " << controller->Name()
           << " calculation time is: " << time_diff_ms << " ms.";
    cmd->mutable_latency_stats()->add_controller_time_ms(time_diff_ms);
  }
  return Status::OK();
}

Status ControlTaskAgent::Reset() {
  for (auto &controller : controller_list_) {
    ADEBUG << "controller:" << controller->Name() << " reset...";
    controller->Reset();
  }
  return Status::OK();
}

}  // namespace control
}  // namespace apollo
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

ControlTask是controller控制器插件的父类，Control/controller控制器插件都继承于ControlTask

control_task.h

/**
 * @file
 * @brief Defines the Controller base class.
 */

#pragma once

#include <memory>
#include <string>

#include <cxxabi.h>

#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
#include "modules/common_msgs/localization_msgs/localization.pb.h"
#include "modules/common_msgs/planning_msgs/planning.pb.h"
#include "modules/control/control_component/proto/calibration_table.pb.h"

#include "cyber/common/file.h"
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/common/status/status.h"
#include "modules/control/control_component/common/control_gflags.h"
#include "modules/control/control_component/controller_task_base/common/dependency_injector.h"

namespace apollo {
namespace control {

class ControlTask {
 public:
  ControlTask() = default;
  virtual ~ControlTask() = default;

  /**
   * @brief initialize Controller
   * @param control_conf control configurations
   * @return Status initialization status
   */
  virtual common::Status Init(std::shared_ptr<DependencyInjector> injector) = 0;

  /**
   * @brief compute control command based on current vehicle status
   *        and target trajectory
   * @param localization vehicle location
   * @param chassis vehicle status e.g., speed, acceleration
   * @param trajectory trajectory generated by planning
   * @param cmd control command
   * @return Status computation status
   */
  virtual common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd) = 0;

  /**
   * @brief reset Controller
   * @return Status reset status
   */
  virtual common::Status Reset() = 0;

  /**
   * @brief controller name
   * @return string controller name in string
   */
  virtual std::string Name() const = 0;

  /**
   * @brief stop controller
   */
  virtual void Stop() = 0;

 protected:
  template <typename T>
  bool LoadConfig(T *config);
  // 加载油门制动标定表
  bool LoadCalibrationTable(calibration_table *calibration_table_conf) {
    std::string calibration_table_path = FLAGS_calibration_table_file;

    if (!apollo::cyber::common::GetProtoFromFile(calibration_table_path,
                                                 calibration_table_conf)) {
      AERROR << "Load calibration table failed!";
      return false;
    }
    AINFO << "Load the calibraiton table file successfully, file path: "
          << calibration_table_path;
    return true;
  }
};

template <typename T>
bool ControlTask::LoadConfig(T *config) {
  int status;
  std::string class_name =
      abi::__cxa_demangle(typeid(*this).name(), 0, 0, &status);
  // Generate the default task config path from PluginManager.
  std::string config_path_ =
      apollo::cyber::plugin_manager::PluginManager::Instance()
          ->GetPluginConfPath<ControlTask>(class_name,
                                           "conf/controller_conf.pb.txt");

  if (!apollo::cyber::common::GetProtoFromFile(config_path_, config)) {
    AERROR << "Load config of " << class_name << " failed!";
    return false;
  }
  AINFO << "Load the [" << class_name
        << "] config file successfully, file path: " << config_path_;
  return true;
}

}  // namespace control
}  // namespace apollo
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

4 controller

接下来看一下具体控制器的实现

---

具体控制器讲解见下一章节