Apollo控制部分1-- ControlComponent组件介绍

- 摘要
- 一、ControlComponent
- - 1、启动文件解析
  - 2、ControlComponent()组件函数解析
  - - 1）ControlComponent::ControlComponent() 构造函数
    - 2）ControlComponent::Init() 初始化函数（执行一次）
    - 3）ControlComponent::Proc() 初始化函数（执行间隔10ms，频率100Hz）
- 二、`⭐`详解
- - `⭐1`：车辆状态信息获取器
  - `⭐2`：参数文件载入
  - - （1）终端配置参数文件
    - （2）程序配置参数文件
  - `⭐3` local_view_解析
- 附表
- - 附表1

@author：Wutong
@time：2023-03-05 15:46

在这里插入图片描述

摘要

本文介绍控制模块入口组件ControlComponent，文件位置为"modules/control/control_component.h"。本文未涉及到控制部分的核心算法，只是讲解Apollo控制模块的最外层包装处理部分，但是读懂这些代码对Apollo整个架构有帮助，能了解到Apollo一些参数载入方式、通用数据处理方式和数据封装方式。控制部分核心算法将在之后的更新中讲解。
ControlComponent的功能为载入参数，处理订阅话题，封装信息并将其传递给子模块处理；
子模块得到路径、底盘、定位、Pad信息计算得到控制量，之后ControlComponent会发布子模块的计算结果控制量信息。

一、ControlComponent

1、启动文件解析

control.launch：launch文件功能为调用control.dag

<cyber>
    <module>
        <name>control</name>
        <dag_conf>/apollo/modules/control/dag/control.dag</dag_conf>
        <process_name>control</process_name>
    </module>
</cyber>

control.dag功能：

启动ControlComponent组件函数（就是启动其生成的动态链接库文件libcontrol_component.so）
flag_file_path参数文件载入（⭐2中会详述Apollo载入参数的方式）
interval: 10（程序每10ms调用一次Proc()函数，细节参考附表一）

module_config {
    module_library : "/apollo/bazel-bin/modules/control/libcontrol_component.so"

    timer_components {
        class_name : "ControlComponent"
        config {
            name: "control"
            flag_file_path: "/apollo/modules/control/conf/control.conf"
            interval: 10
        }
    }
}

2、ControlComponent()组件函数解析

1）ControlComponent::ControlComponent() 构造函数

监视器注册，控制模块出现ERROR由监视器输出

2）ControlComponent::Init() 初始化函数（执行一次）

车辆位置信息，利用此指针获取车辆状态和位置信息（⭐1详解）

#include "modules/control/common/dependency_injector.h"

// 定义共享指针，子程序可以使用指针获取车辆状态信息，如controller_agent_.Init(injector_, &control_conf_)
injector_ = std::make_shared<DependencyInjector>();

// 函数使用，利用订阅的底盘信息和定位信息更新车辆状态信息
injector_->vehicle_state()->Update(local_view->localization(),local_view->chassis());

参数文件载入（⭐2详解）
controller_agent_.Init(injector_, &control_conf_)，初始化子程序
订阅/发布话题：订阅话题包括底盘信息、轨迹信息、定位信息和Pad信息；发布话题包括local_view_信息（⭐3详解）和控制命令信息。
其中，Pad信息包括驾驶模式{人工，自主驾驶等}和驾驶行为{停止，启动，}
睡眠1s，等待定位、规划模块channel消息。

3）ControlComponent::Proc() 初始化函数（执行间隔10ms，频率100Hz）

发布/订阅回调函数调用
ProduceControlCommand()计算控制命令函数
- CheckInput(&local_view_)；若输入数据没有问题，则更新车辆状态获取器信息
- CheckTimestamp(local_view_)：检查数据时间戳是否有问题，监视某一模块是否太久未更新数据
- estop判定：根据输入数据判断是否需要紧急停车。若不需要紧急停车，调用子程序计算控制量
```
controller_agent_.ComputeControlCommand(&local_view_.localization(), &local_view_.chassis(),&local_view_.trajectory(), 	control_command);
```
- 设置车辆灯光信号（根据路径中的灯光信息）local_view_.trajectory().decision().vehicle_signal()
控制消息Header赋值、Latency时延记录
控制命令发送：control_cmd_writer_->Write(control_command);

二、`⭐`详解

`⭐1`：车辆状态信息获取器

功能：将定位数据和底盘数据信息整合成一个新的类，便于子程序调用。比如injector_->vehicle_state()->x()就是从定位信息localization得到的x位置，injector_->vehicle_state()->gear()就是从底盘信息chassis得到的档位信息。
举例（与下面代码注释结合阅读）：

初始化：妈妈为厨房配备了钥匙，这个钥匙就是共享指针injector_ ，厨房就是vehicle_state()，妈妈把钥匙给了我们，我们自己新配了一把钥匙，现在我们可以随意吃厨房里面的东西了。
车辆状态信息更新：妈妈买了水果local_view->localization()和蔬菜local_view->chassis()，对买的东西洗洗涮涮之后injector_->vehicle_state()->Update();。因为我们已有厨房的钥匙，所以可以随便吃里面的水果蔬菜。

#include "modules/control/common/dependency_injector.h"

// 定义共享指针，厨房钥匙
injector_ = std::make_shared< DependencyInjector >();

// 子程序可以使用指针获取车辆状态信息
// 妈妈把钥匙给了我们，我们自己新配了一把钥匙，现在我们可以随意吃厨房里面的东西了
controller_agent_.Init(injector_, &control_conf_)

// 函数调用，利用订阅的底盘信息和定位信息更新车辆状态信息
// 妈妈洗好水果和蔬菜，我们通过之前的钥匙可以随意吃 
injector_->vehicle_state()->Update(local_view->localization(),
                                     local_view->chassis());

class DependencyInjector文件位置’‘modules/control/common/dependency_injector.h’’
举例：根据按照钥匙匹配厨房的过程，没有含金量，厨房内的操作才是重点

#pragma once
#include "modules/common/vehicle_state/vehicle_state_provider.h"

namespace apollo {
namespace control {

class DependencyInjector {
 public:
  DependencyInjector() = default;
  ~DependencyInjector() = default;
  apollo::common::VehicleStateProvider* vehicle_state() {
    return &vehicle_state_;
  }
 private:
  apollo::common::VehicleStateProvider vehicle_state_;
};
}  // namespace control
}

class VehicleStateProvider文件位置：‘‘modules/common/vehicle_state/vehicle_state_provider.h’’
举例：厨房内操作，重点。Update()函数处理定位和底盘信息，相当于厨房洗水果的过程。

class VehicleStateProvider {
 public:
  /* 
  	 利用车辆定位信息和车辆底盘信息更新车辆状态信息
     状态信息 = 定位信息 + 底盘信息
     定位信息：时间戳、位置信息 {x y z}、航向角 heading、角速度 angular_velocity、
  				加速度 linear_acceleration、欧拉角 {roll yaw pitch}
     底盘信息：档位、车速、转向、驾驶模式{人工、完全自动驾驶、仅转向、仅油门刹车}
  */ 
  Status Update(const localization::LocalizationEstimate& localization,
                const canbus::Chassis& chassis);

   // 以当前位置和航向角为计算基准，假设速度、加速度、加速度信息不变
   // 函数功能：预测未来t时刻车辆位置信息
  math::Vec2d EstimateFuturePosition(const double t) const;

  // center of mass(COM) 车辆质心 
  // 定位数据的参照坐标系为后轴，函数通过质心和后轴相对位置计算质心位置
  math::Vec2d ComputeCOMPosition(const double rear_to_com_distance) const;
}

`⭐2`：参数文件载入

控制模块所有使用到的参数信息都可以从下面四种方式之一得到：

终端配置参数文件
1. 全局配置文件定义
2. 控制模块配置文件定义（此种载入方式需要重点掌握）
程序配置参数文件
1. 全局参数
2. 控制模块参数

（1）终端配置参数文件

首先，dag文件中命令flag_file_path: "/apollo/modules/control/conf/control.conf"，所有参数从这里载入，内容如下：
control.conf文件

--flagfile=/apollo/modules/common/data/global_flagfile.txt
--control_conf_file=/apollo/modules/control/conf/control_conf.pb.txt
--enable_speed_station_preview=false
--enable_interpolation_by_time=false
--use_preview_speed_for_table=false
--enable_gain_scheduler=true
--set_steer_limit=true
--enable_slope_offset=false
--enable_maximum_steer_rate_limit=false

--state_transform_to_com_reverse=true
--state_transform_to_com_drive=true
--trajectory_transform_to_com_reverse=true
--trajectory_transform_to_com_drive=true
--enable_feedback_augment_on_high_speed=false
# --reverse_heading_vehicle_state=false
# --reverse_heading_control=false

--query_time_nearest_point_only=false
--query_forward_time_point_only=false
# --use_control_submodules=true

全局配置文件定义：global_flagfile.txt
包括车辆参数配置信息、地图目录等全局信息

--vehicle_config_path=/apollo/modules/common/data/vehicle_param.pb.txt

--log_dir=/apollo/data/log

--use_navigation_mode=false

--map_dir=/apollo/modules/map/data/sunnyvale_loop

--use_sim_time=false

--use_cyber_time=true

--map_dir=/apollo/modules/map/data/sunnyvale

--map_dir=/apollo/modules/map/data/sunnyvale_big_loop

控制模块配置文件定义：control_conf.pb.txt
主要是控制器参数配置信息，包括横向控制器、纵向控制器参数，其通过程序可载入配置文件话题消息

// FLAGS_control_conf_file参数为"/apollo/modules/control/conf/control_conf.pb.txt"
// control_conf_类型为：ControlConf control_conf_;其对应的是"modules/control/proto/control_conf.pb.h"
ACHECK(
      cyber::common::GetProtoFromFile(FLAGS_control_conf_file, &control_conf_))
      << "Unable to load control conf file: " + FLAGS_control_conf_file;

分析"/apollo/modules/control/conf/control_conf.pb.txt"和"modules/control/proto/control_conf.pb"两个文件可以知道其中的参数内容都是一一对应的，Apollo通过调用GetProtoFromFile()函数这样的方式载入参数信息

（2）程序配置参数文件

全局参数：modules/common/adapters/adapter_gflags.h
全局参数是指所有的Apollo程序均使用同样的参数，比如FLAGS_chassis_topic就是指规划、控制等模块都使用"/apollo/canbus/chassis"这个参数；
控制模块参数：modules/control/common/control_gflags.h
控制模块参数是仅控制模块使用的，比如chassis_pending_queue_size 缓冲序列大小控制模块设为10，然而规划等其他模块可以设置20等其他数字。

代码实例：程序配置参数采用Google的gflags方法。定义时采用DECLARE_string(chassis_topic)、DECLARE_int32等声明变量；引用时通过添加FLAG_比如FLAGS_chassis_topic引用变量。

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

modules/common/adapters/adapter_gflags.h

#pragma once
#include "gflags/gflags.h"

DECLARE_string(chassis_topic);
DEFINE_string(chassis_topic, "/apollo/canbus/chassis", "chassis topic name");

modules/control/common/control_gflags.h

#pragma once
#include "gflags/gflags.h"

DECLARE_int32(chassis_pending_queue_size);
DEFINE_int32(chassis_pending_queue_size, 10, "Max chassis pending queue size");

`⭐3` local_view_解析

消息类型定义：modules/control/proto/local_view.proto
可以发现，LocalView 包括了底盘、轨迹、定位和Pad所有的订阅信息，一是为了方便数据的记录，可以将控制模块接收的数据统统通过此话题输出；而是通过一个统一的类管理数据，计算控制命令时，直接将LocalView 赋值给子模块。

syntax = "proto2";

package apollo.control;

import "modules/common_msgs/chassis_msgs/chassis.proto";
import "modules/common_msgs/basic_msgs/header.proto";
import "modules/common_msgs/control_msgs/pad_msg.proto";
import "modules/common_msgs/localization_msgs/localization.proto";
import "modules/common_msgs/planning_msgs/planning.proto";

message LocalView {
  optional apollo.common.Header header = 1;
  optional apollo.canbus.Chassis chassis = 2;
  optional apollo.planning.ADCTrajectory trajectory = 3;
  optional apollo.localization.LocalizationEstimate localization = 4;
  optional PadMessage pad_msg = 5;
}

附表

附表1

interval : 10：Proc()函数调用的时间间隔为10ms

module_config {
    module_library : "/apollo/bazel-bin/cyber/examples/timer_component_example/libtimer_component_example.so"
    timer_components {
        class_name : "TimerComponentSample"
        config {
            name : "timer"
            interval : 10
        }
    }
}

修改cyber/examples/timer_component_example/timer_component_example.cc文件，在Proc()函数中输出函数执行时刻，验证interval为时间间隔

bool TimerComponentSample::Proc() {

  std::cout<<Clock::Now()<<std::endl;
  
  return true;
}

interval:10、interval:100、interval:1000函数输出结果如下所示：

// interval：10
[timer  ]  2023-03-02 17:48:44.938106762
[timer  ]  2023-03-02 17:48:44.948118645
[timer  ]  2023-03-02 17:48:44.958135355
[timer  ]  2023-03-02 17:48:44.968064614
[timer  ]  2023-03-02 17:48:44.978042423

// interval：100
[timer  ]  2023-03-02 17:49:14.709280940
[timer  ]  2023-03-02 17:49:14.809260363
[timer  ]  2023-03-02 17:49:14.909275953
[timer  ]  2023-03-02 17:49:15.008942734
[timer  ]  2023-03-02 17:49:15.109091507

// interval：1000
[timer  ]  2023-03-02 17:49:41.278162660
[timer  ]  2023-03-02 17:49:42.277118315
[timer  ]  2023-03-02 17:49:43.279148986
[timer  ]  2023-03-02 17:49:44.279295854
[timer  ]  2023-03-02 17:49:45.279386135