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

@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. 车辆位置信息，利用此指针获取车辆状态和位置信息（⭐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. 参数文件载入（⭐2详解）
3. controller_agent_.Init(injector_, &control_conf_)，初始化子程序
4. 订阅/发布话题：订阅话题包括底盘信息、轨迹信息、定位信息和Pad信息；发布话题包括local_view_信息（⭐3详解）和控制命令信息。
   其中，Pad信息包括驾驶模式{人工，自主驾驶等}和驾驶行为{停止，启动，}
5. 睡眠1s，等待定位、规划模块channel消息。

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

1. 发布/订阅回调函数调用

2. 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()

3. 控制消息Header赋值、Latency时延记录

4. 控制命令发送：control_cmd_writer_->Write(control_command);

二、⭐详解

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

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

1. 初始化：妈妈为厨房配备了钥匙，这个钥匙就是共享指针injector_ ，厨房就是vehicle_state()，妈妈把钥匙给了我们，我们自己新配了一把钥匙，现在我们可以随意吃厨房里面的东西了。
2. 车辆状态信息更新：妈妈买了水果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

1. 全局配置文件定义：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

2. 控制模块配置文件定义：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