Apollo控制部分2-- ControllerAgent 控制器管理

news2024/11/10 13:01:40

Apollo控制部分2-- ControllerAgent 控制器管理

- 摘要
- 一、预备知识
- - 1、控制模块预备知识
  - 2、多态
- 二、控制器管理解析
- - 1、初始化函数（配置文件参数决定控制器）
  - - 1）配置文件："/apollo/modules/control/conf/control_conf.pb.txt"
    - 2）控制器管理器初始化
    - 3）控制器初始化
  - 2、计算控制命令
- 三、总结

@author：Wutong
@time：2023-03-06 23:48

在这里插入图片描述

摘要

本文介绍了Apollo控制模块所使用的控制方式，包括横向控制器+纵向控制器和MPC控制器两种方式。

横向控制器（LatController） +纵向控制器（LonController）
MPC控制器（MPCController）

之后通过代码介绍了控制器初始化方式、控制器管理方式以及如何通过c++多态特性创建不同控制器对象。

一、预备知识

1、控制模块预备知识

上一讲的知识需要掌握：
Apollo控制部分1-- ControlComponent组件介绍

2、多态

c++多态教程
在c++多态教程中有这样一句话：“有了多态，您可以有多个不同的类，都带有同一个名称但具有不同实现的函数，函数的参数甚至可以是相同的”。这正对应我们的控制器模块，我们有LatController、LonController和MPCController三个不同的类，都带有函数名称相同、函数参数相同、但实现方式不同的函数，比如控制器初始化Init()，计算控制命令ComputeControlCommand()函数。

  // 三个控制器初始化均通过
  virtual common::Status Init(std::shared_ptr<DependencyInjector> injector,
                              const ControlConf *control_conf) = 0;
  
  // 三个控制器均采用相同的函数参数，如定位信息、底盘信息、轨迹信息
  // 输出结果不同：MPC控制器->转角，油门
  // 			Lon控制器->油门		
  // 			Lat控制器->转角
  virtual common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd) = 0;

二、控制器管理解析

1、初始化函数（配置文件参数决定控制器）

1）配置文件：“/apollo/modules/control/conf/control_conf.pb.txt”

功能：active_controllers为控制模块使用的控制器，示例中配置的是横向控制器LAT_CONTROLLER和纵向控制器LON_CONTROLLER，使用横向控制器的转角等结果和纵向控制器的油门等结果作为最终输出。

active_controllers: LAT_CONTROLLER
active_controllers: LON_CONTROLLER

2）控制器管理器初始化

// 此容器保存控制器指针
std::vector<std::unique_ptr<Controller>> controller_list_;
/*
（1）横向控制器+纵向控制器
参数配置文件如下：
active_controllers: LAT_CONTROLLER
active_controllers: LON_CONTROLLER
controller_list_经过上面两个函数处理结果为两个指针（指向横向控制器和指向纵向控制器）

（2）MPC控制器
参数配置文件如下：
active_controllers: MPC_CONTROLLER
controller_list_经过上面两个函数处理结果为一个指针（指向MPC控制器）
*/

通过如下两个函数使用工厂方法创建多态继承类，即创建纵向控制器和横向控制器，并将创建的控制器结果存储在controller_list_

// 注册控制器，根据此配置文件声明两个控制器：LAT_CONTROLLER和LON_CONTROLLER
void ControllerAgent::RegisterControllers(const ControlConf *control_conf){};

// 创建LAT_CONTROLLER和LON_CONTROLLER对象，便于后续使用
Status ControllerAgent::InitializeConf(const ControlConf *control_conf){};

注释：根据配置文件创建控制器对象，代码中通过工厂方式注册和创建多态类较难，如果阅读困难可以直接跳过上面两个函数阅读，直接记住controller_list_结果。

3）控制器初始化

初始化controller_list_中的所有控制器，控制器初始化函数会在之后分析各个控制器时具体分析

for (auto &controller : controller_list_) {
    if (controller == nullptr) {
      return Status(ErrorCode::CONTROL_INIT_ERROR, "Controller is null.");
    }
    if (!controller->Init(injector, control_conf_).ok()) {
      AERROR << "Controller <" << controller->Name() << "> init failed!";
      return Status(ErrorCode::CONTROL_INIT_ERROR,
                    "Failed to init Controller:" + controller->Name());
    }
    AINFO << "Controller <" << controller->Name() << "> init done!";
  }

2、计算控制命令

计算控制命令

（1）横向控制器计算转角等信息，纵向控制器计算油门等信息（二者分别计算）
（2）MPC控制器对油门和转角信息等信息都计算

此示例中controller_list_包含横向控制器和纵向控制器，通过for循环调用两个控制器计算相应的命令。

  // 每个控制器均计算命令
  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);
  }