0 前言
从planning的角度看control,首先需要了解的就是相关的数据接口,规划出的轨迹(路径+速度)发给Control模块去执行
modules/planning/planning_component/planning_component.cc
planning模块发布轨迹信息
planning_writer_ = node_->CreateWriter<ADCTrajectory>(config_.topic_config().planning_trajectory_topic());
modules/control/control_component/control_component.cc
Control模块接受轨迹信息
trajectory_reader_ =node_->CreateReader<ADCTrajectory>(planning_reader_config, nullptr);
ACHECK(trajectory_reader_ != nullptr);
ADCTrajectory
在modules/common_msgs/planning_msgs/planning.proto定义
message ADCTrajectory {optional apollo.common.Header header = 1;optional double total_path_length = 2; // in metersoptional double total_path_time = 3; // in secondsoptional 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 resultoptional apollo.common.Header routing_header = 16;enum RightOfWayStatus {UNPROTECTED = 0;PROTECTED = 1;}optional RightOfWayStatus right_of_way_status = 17;// lane id along current reference linerepeated 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 mostmessage 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 overlapoptional 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 linerepeated apollo.hdmap.Id target_lane_id = 23;// complete dead end flagoptional bool car_in_dead_end = 24;// output related to RSSoptional RSSInfo rss_info = 100;
}
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.5 模块输入输出与配置
输入:
Channel名称 | 类型 | 描述 |
---|---|---|
/apollo/planning | apollo::planning::ADCTrajectory | 车辆规划轨迹线信息 |
/apollo/localization/pose | apollo::localization::LocalizationEstimate | 车辆定位信息 |
/apollo/canbus/chassis | apollo::canbus::Chassis | 车辆底盘信息 |
- | apollo::common::VehicleState | 车身姿态信息 |
/apollo/control/pad | apollo::control::ControlCommand::PadMessage | 自动驾驶使能(请求进入自动驾驶)指令 |
输出:
Channel名称 | 类型 | 描述 |
---|---|---|
/apollo/control | apollo::control::ControlCommand | 车辆的控制指令,如方向盘、油门、刹车等信息 |
配置文件:
文件路径 | 类型/结构 | 说明 |
---|---|---|
modules/control/control_component/conf/pipeline.pb.txt | apollo::control::ControlPipeline | ControlComponent的配置文件 |
modules/control/control_component/conf/control.conf | command line flags | 命令行参数配置 |
modules/control/control_component/conf/calibration_table.pb.txt | apollo::control::calibration_table | 车辆纵向标定表配置 |
Flags:
flagfile | 类型 | 描述 |
---|---|---|
modules/control/control_component/common/control_gflags.cc | flags | Control组件flags变量定义文件 |
modules/control/control_component/common/control_gflags.h | declare | Control组件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_;// 发布者 控制命令、使用控制子模块LocalViewstd::shared_ptr<cyber::Writer<ControlCommand>> control_cmd_writer_;// when using control submodulesstd::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
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 submodulesADEBUG << "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 controllerADEBUG << "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_viewif (!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// 休眠1000msAINFO << "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 dataif (!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;}}// 检查 estopestop_ = 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 agentStatus status_compute = control_task_agent_.ComputeControlCommand(&local_view_.localization(), &local_view_.chassis(),&local_view_.trajectory(), control_command);ADEBUG << "status_compute is " << status_compute;// 如果计算控制命令失败,记录错误信息并设置 estop_ 为 trueif (!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 commandcontrol_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 signalif (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 copystd::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 submodulesif (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 latencystatic 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 headercontrol_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 latencyif (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
控制全局变量配置文件见control_gflags.cc
配置加载的控制器,Apollo中modules/control/control_component/conf/pipeline.pb.txt
controller {name: "LAT_CONTROLLER"type: "LatController"
}
controller {name: "LON_CONTROLLER"type: "LonController"
}
name是用户自定义,表达清楚是什么控制器就行,type是控制器的子类名称,如果和子类名称不一致,会导致加载控制器失败。上面是先加载横向控制器,再加载纵向控制器
3 Control组件包逻辑梳理
主要是梳理Init函数和Proc函数
Init函数
:
主要实现ControlTaskAgent
的初始化,以及control上游的相关消息的订阅
摘取Init函数里面的主要部分
bool ControlComponent::Init() {// 初始化控制器agentif (!FLAGS_is_control_ut_test_mode) {if (!FLAGS_use_control_submodules &&!control_task_agent_.Init(injector_, control_pipeline_).ok()) {// set controllerADEBUG << "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);
}
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);
}
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*/// 重置ControlTaskAgentcommon::Status Reset();private:std::vector<std::shared_ptr<ControlTask>> controller_list_;std::shared_ptr<DependencyInjector> injector_ = nullptr;
};} // namespace control
} // namespace apollo
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
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
4 controller
接下来看一下具体控制器的实现
具体控制器讲解见下一章节