目录结构介绍
cyber 消息中间件，替换ros作为消息层
data 地图等生成好的数据放在这里（其他数据待补充）
docker 容器相关
docs 文档相关
modules 定位，预测，感知，规划等自动驾驶模块
scripts 脚本
third_party 第三方库
tools 工具目录，几乎为空
Cyber模块
Cyber main函数中先根据命令行信息解析dag参数，然后利用解析的参数，动态的加载对应的模块
输入：命令行数量+命令行内容
输出：加载状态
关联模块：All
mainboard.cc
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "cyber/common/global_data.h"
#include "cyber/common/log.h"
#include "cyber/init.h"
#include "cyber/mainboard/module_argument.h"
#include "cyber/mainboard/module_controller.h"
#include "cyber/state.h"
using apollo::cyber::mainboard::ModuleArgument;
using apollo::cyber::mainboard::ModuleController;
int main(int argc, char** argv)//命令行参数的数量与内容
{
//解析参数
ModuleArgument module_args;
1.module_args.ParseArgument(argc, argv);//输出解析结果
//初始化环境
apollo::cyber::Init(argv[0]);
ModuleController controller(module_args);
//Init中启动LoadAll，若启动失败
3.if (!controller.Init())
{
controller.Clear();
AERROR << "module start error.";
return -1;
}
//等待关闭
apollo::cyber::WaitForShutdown();
//卸载模块
controller.Clear();
AINFO << "exit mainboard.";
return 0;
}
ParseArgument(输出解析信息)
void ModuleArgument::ParseArgument(const int argc, char* const argv[])
{
binary_name_ = std::string(basename(argv[0]));
//解析命令行参数，根据不同的选项设置相应的变量值，以供程序后续使用(存入dag_conf_list)
2.GetOptions(argc, argv);
if (process_group_.empty())
{
process_group_ = DEFAULT_process_group_;
}
if (sched_name_.empty())
{
sched_name_ = DEFAULT_sched_name_;
}
//设定process_group和sched_name，若没有则为默认值
GlobalData::Instance()->SetProcessGroup(process_group_);
GlobalData::Instance()->SetSchedName(sched_name_);
AINFO << "binary_name_ is " << binary_name_ << ", process_group_ is "
<< process_group_ << ", has " << dag_conf_list_.size() << " dag conf";
//从dag_conf_list读取字符串到dag中并打印
for (std::string& dag : dag_conf_list_)
{
AINFO << "dag_conf: " << dag;
}
}
GetOptions(获取选项设定值)
void ModuleArgument::GetOptions(const int argc, char* const argv[]) {
opterr = 0;//外部全局变量
int long_index = 0;
//定义长短选项，根据输入选择不同操作
const std::string short_opts = "hd:p:s:";
static const struct option long_opts[] = {
{"help", no_argument, nullptr, 'h'},
{"dag_conf", required_argument, nullptr, 'd'},
{"process_name", required_argument, nullptr, 'p'},
{"sched_name", required_argument, nullptr, 's'},
{NULL, no_argument, nullptr, 0}};
//命令记录
std::string cmd("");
for (int i = 0; i < argc; ++i) {
cmd += argv[i];
cmd += " ";
}
AINFO << "command: " << cmd;
if (1 == argc) {
DisplayUsage();
exit(0);
}
//根据类型执行
do {
int opt =getopt_long(argc, argv, short_opts.c_str(), long_opts, &long_index);
if (opt == -1) {
break;
}
switch (opt) {
//存储路径
case 'd':
dag_conf_list_.emplace_back(std::string(optarg));
for (int i = optind; i < argc; i++) {
if (*argv[i] != '-') {
dag_conf_list_.emplace_back(std::string(argv[i]));
} else {
break;
}
}
break;
case 'p':
process_group_ = std::string(optarg);
break;
case 's':
sched_name_ = std::string(optarg);
break;
case 'h':
DisplayUsage();
exit(0);
default:
break;
}
} while (true);
if (optind < argc) {
AINFO << "Found non-option ARGV-element \"" << argv[optind++] << "\"";
DisplayUsage();
exit(1);
}
if (dag_conf_list_.empty()) {
AINFO << "-d parameter must be specified";
DisplayUsage();
exit(1);
}
}
LoadAll(加载所有模块)
bool ModuleController::LoadAll()//加载所有模块(生成路径，调用)
{
//获取根目录
4.const std::string work_root = common::WorkRoot();
//获取当前工作目录
const std::string current_path = common::GetCurrentPath();
//获取绝对路径
const std::string dag_root_path = common::GetAbsolutePath(work_root, "dag");
std::vector<std::string> paths;
for (auto& dag_conf : args_.GetDAGConfList())
{
std::string module_path = "";
if (dag_conf == common::GetFileName(dag_conf))
{
//dag_conf为文件名，则拼接路径
module_path = common::GetAbsolutePath(dag_root_path, dag_conf);
} else if (dag_conf[0] == '/')
{
//dag_conf为绝对路径
module_path = dag_conf;
} else
{
//dag_conf为相对路径
module_path = common::GetAbsolutePath(current_path, dag_conf);
if (!common::PathExists(module_path))
{
module_path = common::GetAbsolutePath(work_root, dag_conf);
}
}
total_component_nums += GetComponentNum(module_path);
paths.emplace_back(std::move(module_path));
}
if (has_timer_component)
{
total_component_nums += scheduler::Instance()->TaskPoolSize();
}
//将结果转变为全局变量
common::GlobalData::Instance()->SetComponentNums(total_component_nums);
for (auto module_path : paths)
{
AINFO << "Start initialize dag: " << module_path;
//加载模块失败
if (!LoadModule(module_path))
{
AERROR << "Failed to load module: " << module_path;
return false;
}
}
return true;
}
WorkRoot(获取根目录)
inline const std::string WorkRoot()
{
//检索环境变量CYBER_PATH
std::string work_root = GetEnv("CYBER_PATH");
if (work_root.empty())
{
work_root = "/apollo/cyber";
}
return work_root;
}
Map模块
去年的文件结构如上所示，8.0版本去除了proto部分，所以地图各元素的消息格式暂时未知
Localization模块
Localization模块主要实现车辆的位置（Planning模块）与车辆的姿态、速度信息（Control模块)
输入：IMU与GPS传感器信息
输出:Localization实例
关联模块：Planning+Control
rtk_localization.cc(待补：rtk_localization_component.cc)
void RTKLocalization::GpsCallback(
const std::shared_ptr<localization::Gps> &gps_msg) {
//获取延迟
double time_delay = last_received_timestamp_sec_
? Clock::NowInSeconds() - last_received_timestamp_sec_
: last_received_timestamp_sec_;
//超出时间则提出警告
if (time_delay > gps_time_delay_tolerance_) {
std::stringstream ss;
ss << "GPS message time interval: " << time_delay;
monitor_logger_.WARN(ss.str());
}
//处理错误
{
//互斥访问惯性信息
std::unique_lock<std::mutex> lock(imu_list_mutex_);
if (imu_list_.empty()) {
AERROR << "IMU message buffer is empty.";
if (service_started_) {
monitor_logger_.ERROR("IMU message buffer is empty.");
}
return;
}
}
{
//互斥访问GPS状态信息
std::unique_lock<std::mutex> lock(gps_status_list_mutex_);
if (gps_status_list_.empty()) {
AERROR << "Gps status message buffer is empty.";
if (service_started_) {
monitor_logger_.ERROR("Gps status message buffer is empty.");
}
return;
}
}
//发布定位信息
1. PrepareLocalizationMsg(*gps_msg, &last_localization_result_,&last_localization_status_result_);
service_started_ = true;
if (service_started_time == 0.0) {
service_started_time = Clock::NowInSeconds();
}
//监视运行状况
RunWatchDog(gps_msg->header().timestamp_sec());
last_received_timestamp_sec_ = Clock::NowInSeconds();
}
PrepareLocalizationMsg(准备建定位消息)
void RTKLocalization::PrepareLocalizationMsg(const localization::Gps &gps_msg, LocalizationEstimate *localization,LocalizationStatus *localization_status)
{
//寻找匹配的GPS和IMU消息
double gps_time_stamp = gps_msg.header().timestamp_sec();
CorrectedImu imu_msg;
2.FindMatchingIMU(gps_time_stamp, &imu_msg);
3.ComposeLocalizationMsg(gps_msg, imu_msg, localization);//组成定位信息
drivers::gnss::InsStat gps_status;
FindNearestGpsStatus(gps_time_stamp, &gps_status);//寻找最近的GPS状态
4.FillLocalizationStatusMsg(gps_status, localization_status);
}
FindMatchingIMU(寻找匹配的IMU)
bool RTKLocalization::FindMatchingIMU(const double gps_timestamp_sec,CorrectedImu *imu_msg) {
if (imu_msg == nullptr) {
AERROR << "imu_msg should NOT be nullptr.";
return false;
}
std::unique_lock<std::mutex> lock(imu_list_mutex_);
auto imu_list = imu_list_;
lock.unlock();
if (imu_list.empty()) {
AERROR << "Cannot find Matching IMU. "<< "IMU message Queue is empty! GPS timestamp[" << gps_timestamp_sec<< "]";
return false;
}
//扫描IMU缓冲区,寻找时间戳更新的消息
auto imu_it = imu_list.begin();
for (; imu_it != imu_list.end(); ++imu_it) {
if ((*imu_it).header().timestamp_sec() - gps_timestamp_sec >std::numeric_limits<double>::min()) {
break;
}
}
//目标存在
if (imu_it != imu_list.end()) {
if (imu_it == imu_list.begin()) {
AERROR << "IMU queue too short or request too old. "
<< "Oldest timestamp[" << imu_list.front().header().timestamp_sec()
<< "], Newest timestamp["
<< imu_list.back().header().timestamp_sec() << "], GPS timestamp["
<< gps_timestamp_sec << "]";
*imu_msg = imu_list.front();
}
else {
//正常情况
auto imu_it_1 = imu_it;
imu_it_1--;
if (!(*imu_it).has_header() || !(*imu_it_1).has_header()) {
AERROR << "imu1 and imu_it_1 must both have header.";
return false;
}
//插值，同步imu与gps
if (!InterpolateIMU(*imu_it_1, *imu_it, gps_timestamp_sec, imu_msg)) {
AERROR << "failed to interpolate IMU";
return false;
}
}
}
else {
//所有数据时间戳都在请求时间戳之前，则提供最新的IMU数据，不进行外推
*imu_msg = imu_list.back();
if (imu_msg == nullptr) {
AERROR << "Fail to get latest observed imu_msg.";
return false;
}
if (!imu_msg->has_header()) {
AERROR << "imu_msg must have header.";
return false;
}
if (std::fabs(imu_msg->header().timestamp_sec() - gps_timestamp_sec) > gps_imu_time_diff_threshold_) {
//报错阈值为20ms
AERROR << "Cannot find Matching IMU. IMU messages too old. "
<< "Newest timestamp[" << imu_list.back().header().timestamp_sec()
<< "], GPS timestamp[" << gps_timestamp_sec << "]";
}
}
return true;
}
ComposeLocalizationMsg（获取Localization并坐标变换）
void RTKLocalization::ComposeLocalizationMsg(const localization::Gps &gps_msg, const localization::CorrectedImu &imu_msg,LocalizationEstimate *localization) {
localization->Clear();
FillLocalizationMsgHeader(localization);
localization->set_measurement_time(gps_msg.header().timestamp_sec());
//结合GPS与IMU
auto mutable_pose = localization->mutable_pose();
if (gps_msg.has_localization()) {
const auto &pose = gps_msg.localization();
if (pose.has_position()) {
//position为localization的一部分，以下代码可实现world frame -> map frame
mutable_pose->mutable_position()->set_x(pose.position().x() -map_offset_[0]);
mutable_pose->mutable_position()->set_y(pose.position().y() -map_offset_[1]);
mutable_pose->mutable_position()->set_z(pose.position().z() -map_offset_[2]);
}
//orientation，四元数转化为航向角
if (pose.has_orientation()) {
mutable_pose->mutable_orientation()->CopyFrom(pose.orientation());
double heading = common::math::QuaternionToHeading(pose.orientation().qw(), pose.orientation().qx(),pose.orientation().qy(), pose.orientation().qz());
mutable_pose->set_heading(heading);
}
//线速度
if (pose.has_linear_velocity()) {
mutable_pose->mutable_linear_velocity()->CopyFrom(pose.linear_velocity());
}
}
if (imu_msg.has_imu()) {
const auto &imu = imu_msg.imu();
//线加速度
if (imu.has_linear_acceleration()) {
if (localization->pose().has_orientation()) {
//将车辆坐标系中的位置信息转换为地图坐标系中的位置信息
Vector3d orig(imu.linear_acceleration().x(),imu.linear_acceleration().y(),imu.linear_acceleration().z());
Vector3d vec = common::math::QuaternionRotate(localization->pose().orientation(), orig);
mutable_pose->mutable_linear_acceleration()->set_x(vec[0]);
mutable_pose->mutable_linear_acceleration()->set_y(vec[1]);
mutable_pose->mutable_linear_acceleration()->set_z(vec[2]);
//车辆坐标系线加速度
mutable_pose->mutable_linear_acceleration_vrf()->CopyFrom(imu.linear_acceleration());
}
else {
AERROR << "[PrepareLocalizationMsg]: "<< "fail to convert linear_acceleration";
}
}
//角速度ω
if (imu.has_angular_velocity()) {
if (localization->pose().has_orientation()) {
//convert from vehicle reference to map reference(同上)
Vector3d orig(imu.angular_velocity().x(), imu.angular_velocity().y(),
imu.angular_velocity().z());
Vector3d vec = common::math::QuaternionRotate(
localization->pose().orientation(), orig);
mutable_pose->mutable_angular_velocity()->set_x(vec[0]);
mutable_pose->mutable_angular_velocity()->set_y(vec[1]);
mutable_pose->mutable_angular_velocity()->set_z(vec[2]);
//车辆坐标系角加速度
mutable_pose->mutable_angular_velocity_vrf()->CopyFrom(
imu.angular_velocity());
} else {
AERROR << "[PrepareLocalizationMsg]: fail to convert angular_velocity";
}
}
//欧拉角
if (imu.has_euler_angles()) {
mutable_pose->mutable_euler_angles()->CopyFrom(imu.euler_angles());
}
}
}
FillLocalizationStatusMsg(填充Localization中的GNSS状态)
void RTKLocalization::FillLocalizationStatusMsg(const drivers::gnss::InsStat &status,LocalizationStatus *localization_status) {
apollo::common::Header *header = localization_status->mutable_header();
double timestamp = apollo::cyber::Clock::NowInSeconds();
header->set_timestamp_sec(timestamp);
localization_status->set_measurement_time(status.header().timestamp_sec());
//检查是否包含位置信息
if (!status.has_pos_type()) {
localization_status->set_fusion_status(MeasureState::ERROR);
localization_status->set_state_message("Error: Current Localization Status Is Missing.");
return;
}
//根据GNSS(GPS)的不同解决方案设置状态
auto pos_type = static_cast<drivers::gnss::SolutionType>(status.pos_type());
switch (pos_type) {
case drivers::gnss::SolutionType::INS_RTKFIXED://差分固定解，亚厘米级精度
localization_status->set_fusion_status(MeasureState::OK);
localization_status->set_state_message("");
break;
case drivers::gnss::SolutionType::INS_RTKFLOAT://差分浮动解，无法稳定提供差分固定解时的方案
localization_status->set_fusion_status(MeasureState::WARNNING);
localization_status->set_state_message("Warning: Current Localization Is Unstable.");
break;
default:
localization_status->set_fusion_status(MeasureState::ERROR);
localization_status->set_state_message("Error: Current Localization Is Very Unstable.");
break;
}
}
Planning模块
Planning模块的作用是根据感知(Prediction)预测的结果，当前的车辆信息和路况规划出一条车辆能够行驶的轨迹，继而交给控制(Control)模块执行
输入：
- 预测的障碍物信息(prediction_obstacles)
- 车辆底盘(Chassis)信息(车辆的速度，加速度，航向角等信息)
- 车辆当前位置(localization_estimate)
输出:规划的路径
关联模块：Control+Localization+Prediction+Routing
planning_component.cc
bool PlanningComponent::Init() {
//两种模式注册
injector_ = std::make_shared<DependencyInjector>();
if (FLAGS_use_navigation_mode) {
planning_base_ = std::make_unique<NaviPlanning>(injector_);//导航级别路径规划
} else {
planning_base_ = std::make_unique<OnLanePlanning>(injector_);//车道级别路径规划
}
//配置文件加载
ACHECK(ComponentBase::GetProtoConfig(&config_))
<< "failed to load planning config file "
<< ComponentBase::ConfigFilePath();
if (FLAGS_planning_offline_learning ||config_.learning_mode() != PlanningConfig::NO_LEARNING) {
if (!message_process_.Init(config_, injector_)) {
AERROR << "failed to init MessageProcess";
return false;
}
}
planning_base_->Init(config_);
//订阅Routing模块信息
routing_reader_ = node_->CreateReader<RoutingResponse>(
config_.topic_config().routing_response_topic(),
[this](const std::shared_ptr<RoutingResponse>& routing) {
AINFO << "Received routing data: run routing callback."
<< routing->header().DebugString();
std::lock_guard<std::mutex> lock(mutex_);
routing_.CopyFrom(*routing);
});
//读取红绿灯信息
traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
config_.topic_config().traffic_light_detection_topic(),
[this](const std::shared_ptr<TrafficLightDetection>& traffic_light) {
ADEBUG << "Received traffic light data: run traffic light callback.";
std::lock_guard<std::mutex> lock(mutex_);
traffic_light_.CopyFrom(*traffic_light);
});
//订阅pad消息
pad_msg_reader_ = node_->CreateReader<PadMessage>(
config_.topic_config().planning_pad_topic(),
[this](const std::shared_ptr<PadMessage>& pad_msg) {
ADEBUG << "Received pad data: run pad callback.";
std::lock_guard<std::mutex> lock(mutex_);
pad_msg_.CopyFrom(*pad_msg);
});
//订阅Story_telling模块信息
story_telling_reader_ = node_->CreateReader<Stories>(
config_.topic_config().story_telling_topic(),
[this](const std::shared_ptr<Stories>& stories) {
ADEBUG << "Received story_telling data: run story_telling callback.";
std::lock_guard<std::mutex> lock(mutex_);
stories_.CopyFrom(*stories);
});
//若使用导航级别路径规划，则读取相对地图
if (FLAGS_use_navigation_mode) {
relative_map_reader_ = node_->CreateReader<MapMsg>(
config_.topic_config().relative_map_topic(),
[this](const std::shared_ptr<MapMsg>& map_message) {
ADEBUG << "Received relative map data: run relative map callback.";
std::lock_guard<std::mutex> lock(mutex_);
relative_map_.CopyFrom(*map_message);
});
}
//发布规划好的线路
planning_writer_ = node_->CreateWriter<ADCTrajectory>(config_.topic_config().planning_trajectory_topic());
//发布重新Routing请求
rerouting_writer_ = node_->CreateWriter<RoutingRequest>(config_.topic_config().routing_request_topic());
//发布规划学习数据（?）
planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(config_.topic_config().planning_learning_data_topic());
return true;
}
bool PlanningComponent::Proc(
const std::shared_ptr<prediction::PredictionObstacles>&prediction_obstacles,
const std::shared_ptr<canbus::Chassis>& chassis,
const std::shared_ptr<localization::LocalizationEstimate>&localization_estimate) {
ACHECK(prediction_obstacles != nullptr);
//检查并执行可能的重新Routing请求
CheckRerouting();
//处理融合后输入数据
local_view_.prediction_obstacles = prediction_obstacles;
local_view_.chassis = chassis;
local_view_.localization_estimate = localization_estimate;
//更新Routing状态，数据放入local_view
{
std::lock_guard<std::mutex> lock(mutex_);
if (!local_view_.routing ||hdmap::PncMap::IsNewRouting(*local_view_.routing, routing_)) {
local_view_.routing =std::make_shared<routing::RoutingResponse>(routing_);
}
}
{
std::lock_guard<std::mutex> lock(mutex_);
local_view_.traffic_light =std::make_shared<TrafficLightDetection>(traffic_light_);
local_view_.relative_map = std::make_shared<MapMsg>(relative_map_);
}
{
std::lock_guard<std::mutex> lock(mutex_);
local_view_.pad_msg = std::make_shared<PadMessage>(pad_msg_);
}
{
std::lock_guard<std::mutex> lock(mutex_);
local_view_.stories = std::make_shared<Stories>(stories_);
}
if (!CheckInput()) {
AERROR << "Input check failed";
return false;
}
//在线训练的数据处理
if (config_.learning_mode() != PlanningConfig::NO_LEARNING) {
message_process_.OnChassis(*local_view_.chassis);
message_process_.OnPrediction(*local_view_.prediction_obstacles);
message_process_.OnRoutingResponse(*local_view_.routing);
message_process_.OnStoryTelling(*local_view_.stories);
message_process_.OnTrafficLightDetection(*local_view_.traffic_light);
message_process_.OnLocalization(*local_view_.localization_estimate);
}
//发布用于RL测试的数据
if (config_.learning_mode() == PlanningConfig::RL_TEST) {
PlanningLearningData planning_learning_data;
LearningDataFrame* learning_data_frame =injector_->learning_based_data()->GetLatestLearningDataFrame();
if (learning_data_frame) {
planning_learning_data.mutable_learning_data_frame()
->CopyFrom(*learning_data_frame);
common::util::FillHeader(node_->Name(), &planning_learning_data);
planning_learning_data_writer_->Write(planning_learning_data);
} else {
AERROR << "fail to generate learning data frame";
return false;
}
return true;
}
//执行注册好的Planning，生成线路
ADCTrajectory adc_trajectory_pb;
//生成一次路径，将结果存储在adc_trajectory_pb中
planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
auto start_time = adc_trajectory_pb.header().timestamp_sec();
common::util::FillHeader(node_->Name(), &adc_trajectory_pb);
//根据header时间戳的变化修改轨迹相对时间
const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
p.set_relative_time(p.relative_time() + dt);
}
//发布生成的路径
planning_writer_->Write(adc_trajectory_pb);
//加入历史记录
auto* history = injector_->history();
history->Add(adc_trajectory_pb);
return true;
}
Control模块
Control模块的作用是根据规划(Planning)模块生成的轨迹，计算出汽车的油门，刹车和方向盘信号，控制汽车按照规定的轨迹行驶
输入：车辆底盘(Chassis)信息, 位置信息(LocalizationEstimate), Planning模块规划的轨迹(ADCTrajectory)
输出：油门、刹车、方向盘控制(ControlCommand)
关联模块：Planning+Localization
control_component.cc
bool ControlComponent::Init() {
//订阅Chassis信息
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);
//订阅Planning信息
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);
//订阅Localization信息
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);
}
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!";
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!";
return false;
}
//为什么判断？？？
if (latest_trajectory_.header().sequence_num() !=trajectory_msg->header().sequence_num()) {
OnPlanning(trajectory_msg);
}
localization_reader_->Observe();
const auto &localization_msg = localization_reader_->GetLatestObserved();
if (localization_msg == nullptr) {
AERROR << "localization msg is not ready!";
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):避免冗余拷贝
{
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_);
}
}
//若使用Control子模块
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();
//测量延迟
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;
}
//处理pad_msg
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 (control_conf_.is_control_test_mode() &&control_conf_.control_test_duration() > 0 &&
(start_time - init_time_).ToSecond() >control_conf_.control_test_duration()) {
AERROR << "Control finished testing. exit";
return false;
}*/
//生成控制命令
ControlCommand control_command;
Status status;
if (local_view_.chassis().driving_mode() == apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
1.status = ProduceControlCommand(&control_command);
} else {
//重置车辆控制状态
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;
}
//在Control框架下发布紧急制动原因并设置头部信息
if (estop_) {
control_command.mutable_header()->mutable_status()->set_msg(estop_reason_);
}
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 (control_conf_.is_control_test_mode()) {
ADEBUG << "Skip publish control command in test mode";
return true;
}
//计算时间开销(ms为单位)
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 > control_conf_.control_period() * 1e3);
ADEBUG << "control cycle time is: " << time_diff_ms << " ms.";
status.Save(control_command.mutable_header()->mutable_status());
//测量延迟
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);
}
//发布控制命令
control_cmd_writer_->Write(control_command);
return true;
}
ProduceControlCommand(计算控制参数并执行)
Status ControlComponent::ProduceControlCommand(ControlCommand *control_command) {
Status status = CheckInput(&local_view_);
//检查数据，错误则紧急停车，转换为手动控制
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 {
//检查时间戳，超时
Status status_ts = CheckTimestamp(local_view_);
if (!status_ts.ok()) {
AERROR << "Input messages timeout";
//如果没有接收到新的数据，则清空轨迹信息，停止自动控制
trajectory_reader_->ClearData();
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_ = control_conf_.enable_persistent_estop()
? estop_ || local_view_.trajectory().estop().is_estop()
: local_view_.trajectory().estop().is_estop();
if (local_view_.trajectory().estop().is_estop()) {
estop_ = true;
estop_reason_ = "estop from planning : ";
estop_reason_ += local_view_.trajectory().estop().reason();
}
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();
}
//车辆处于驾驶档且速度为负值，则触发
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) {
controller_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_);
}
//控制器代理(核心部分)，利用传入的Localization、chassis、trajectory信息计算控制参数
Status status_compute = controller_agent_.ComputeControlCommand(&local_view_.localization(), &local_view_.chassis(),&local_view_.trajectory(), control_command);
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;
}
}
//如果Planning模块触发了紧急制动,则不会执行Control命令
if (estop_) {
AWARN_EVERY(100) << "Estop triggered! No control core method executed!";
control_command->set_speed(0);
control_command->set_throttle(0);
control_command->set_brake(control_conf_.soft_estop_brake());
control_command->set_gear_location(Chassis::GEAR_DRIVE);
}
//将signal信息(Planning？)复制到Control模块
if (local_view_.trajectory().decision().has_vehicle_signal()) {
control_command->mutable_signal()->CopyFrom(local_view_.trajectory().decision().vehicle_signal());
}
return status;
}
pad_msg主要包括以下两种功能：
- 发送消息给Canbus模块，来控制driving_mode，Control模块根据当前driving_mode的状态来判断是否启动自动驾驶(pad_msg由Planning模块订阅并发布)
- 通过reset来清空estop_的状态(estop_其中一个产生条件为路径为空，起源于Planning模块)
Perception模块
Perception模块利用各种传感器数据中获取的有关环境信息，使得车辆能够理解、感知其周围的事物
输入：雷达与图像数据，车辆的速度与角速度
输出：具有航向、速度和分类信息的3D障碍物轨迹以及交通信号灯检测和识别的输出
关联模块：Prediction+Localization
Prediction模块
Prediction模块主要研究并预测感知模块(Perception)检测到的所有障碍物的行为，预测接收障碍物数据以及基本感知信息，包括位置、航向、速度、加速度，然后生成具有这些障碍物概率的预测轨迹
输入：感知模块的障碍物信息(Perception)，定位模块的定位信息(Localization)，规划模块的前一个计算周期的规划轨迹(Planning)
输出：具有预测轨迹的障碍物，障碍物标注有预测轨迹及其优先级
关联模块：Perception+Localization+Planning*
prediction_component.cc
void PredictionComponent::OfflineProcessFeatureProtoFile(const std::string& features_proto_file_name) {
//读取障碍物容器指针
auto obstacles_container_ptr = container_manager_->GetContainer<ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
obstacles_container_ptr->Clear();
//将特征加入障碍物容器(Perception)
Features features;
apollo::cyber::common::GetProtoFromBinaryFile(features_proto_file_name,&features);
for (const Feature& feature : features.feature()) {
obstacles_container_ptr->InsertFeatureProto(feature);
Obstacle* obstacle_ptr = obstacles_container_ptr->GetObstacle(feature.id());
if (!obstacle_ptr) {
continue;
}
//评估障碍物?
1.evaluator_manager_->EvaluateObstacle(obstacle_ptr, obstacles_container_ptr);
}
}
bool PredictionComponent::Init() {
component_start_time_ = Clock::NowInSeconds();
container_manager_ = std::make_shared<ContainerManager>();
evaluator_manager_.reset(new EvaluatorManager());
predictor_manager_.reset(new PredictorManager());
scenario_manager_.reset(new ScenarioManager());
PredictionConf prediction_conf;
//未能加载配置文件
if (!ComponentBase::GetProtoConfig(&prediction_conf)) {
AERROR << "Unable to load prediction conf file: " << ComponentBase::ConfigFilePath();
return false;
}
ADEBUG << "Prediction config file is loaded into: " << prediction_conf.ShortDebugString();
if (!MessageProcess::Init(container_manager_.get(), evaluator_manager_.get(), predictor_manager_.get(), prediction_conf)) {
return false;
}
//订阅各模块信息
planning_reader_ = node_->CreateReader<ADCTrajectory>(prediction_conf.topic_conf().planning_trajectory_topic(), nullptr);
localization_reader_ = node_->CreateReader<localization::LocalizationEstimate>(prediction_conf.topic_conf().localization_topic(), nullptr);
storytelling_reader_ = node_->CreateReader<storytelling::Stories>(prediction_conf.topic_conf().storytelling_topic(), nullptr);
prediction_writer_ = node_->CreateWriter<PredictionObstacles>(prediction_conf.topic_conf().prediction_topic());
container_writer_ = node_->CreateWriter<SubmoduleOutput>(prediction_conf.topic_conf().container_topic_name());
adc_container_writer_ = node_->CreateWriter<ADCTrajectoryContainer>(prediction_conf.topic_conf().adccontainer_topic_name());
perception_obstacles_writer_ = node_->CreateWriter<PerceptionObstacles>(prediction_conf.topic_conf().perception_obstacles_topic_name());
return true;
}
bool PredictionComponent::Proc(const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
if (FLAGS_use_lego) {
//将各模块内容处理后，放入容器供其他部分使用
3.return ContainerSubmoduleProcess(perception_obstacles);
}
//涉及更多的数据处理和后续任务，发布预测的障碍物信息
4.return PredictionEndToEndProc(perception_obstacles);
}
EvaluateObstacle(根据障碍物选择评估器)
void EvaluatorManager::EvaluateObstacle(const ADCTrajectoryContainer* adc_trajectory_container,
Obstacle* obstacle,ObstaclesContainer* obstacles_container,std::vector<Obstacle*> dynamic_env) {
Evaluator* evaluator = nullptr;
//根据障碍物的类型选择不同的评估器(evaluators)
switch (obstacle->type()) {
case PerceptionObstacle::VEHICLE: {
//警告却不是慢行(潜在危险)
if (obstacle->IsCaution() && !obstacle->IsSlow()) {
if (obstacle->IsInteractiveObstacle()) {
evaluator = GetEvaluator(interaction_evaluator_);
} else if (obstacle->IsNearJunction()) {
evaluator = GetEvaluator(vehicle_in_junction_caution_evaluator_);
} else if (obstacle->IsOnLane()) {
evaluator = GetEvaluator(vehicle_on_lane_caution_evaluator_);
} else {
evaluator = GetEvaluator(vehicle_default_caution_evaluator_);
}
CHECK_NOTNULL(evaluator);
//评估并退出
if (evaluator->GetName() == "JOINTLY_PREDICTION_PLANNING_EVALUATOR") {
2.if (evaluator->Evaluate(adc_trajectory_container,obstacle, obstacles_container)) {
break;
} else {
AERROR << "Obstacle: " << obstacle->id() << " interaction evaluator failed," << " downgrade to normal level!";
}
} else {
if (evaluator->Evaluate(obstacle, obstacles_container)) {
break;
} else {
AERROR << "Obstacle: " << obstacle->id() << " caution evaluator failed, downgrade to normal level!";
}
}
}
//障碍物不属于警告类别或警告评估失败
if (obstacle->HasJunctionFeatureWithExits() &&!obstacle->IsCloseToJunctionExit()) {
//障碍物在交叉路口里
evaluator = GetEvaluator(vehicle_in_junction_evaluator_);
} else if (obstacle->IsOnLane()) {
evaluator = GetEvaluator(vehicle_on_lane_evaluator_);
} else {
//不再评估并退出
ADEBUG << "Obstacle: " << obstacle->id() << " is neither on lane, nor in junction. Skip evaluating.";
break;
}
CHECK_NOTNULL(evaluator);
if (evaluator->GetName() == "LANE_SCANNING_EVALUATOR") {
evaluator->Evaluate(obstacle, obstacles_container, dynamic_env);
} else {
evaluator->Evaluate(obstacle, obstacles_container);
}
break;
}
case PerceptionObstacle::BICYCLE: {
if (obstacle->IsOnLane()) {
evaluator = GetEvaluator(cyclist_on_lane_evaluator_);
CHECK_NOTNULL(evaluator);
evaluator->Evaluate(obstacle, obstacles_container);
}
break;
}
case PerceptionObstacle::PEDESTRIAN: {
//若处于离线模式或者优先级为CAUTION，则进行评估
if (FLAGS_prediction_offline_mode == PredictionConstants::kDumpDataForLearning ||
obstacle->latest_feature().priority().priority() == ObstaclePriority::CAUTION) {
evaluator = GetEvaluator(pedestrian_evaluator_);
CHECK_NOTNULL(evaluator);
evaluator->Evaluate(obstacle, obstacles_container);
break;
}
}
default: {
if (obstacle->IsOnLane()) {
evaluator = GetEvaluator(default_on_lane_evaluator_);
CHECK_NOTNULL(evaluator);
evaluator->Evaluate(obstacle, obstacles_container);
}
break;
}
}
}
PedestrianInteractionEvaluator::Evaluate(使用深度学习模型进行预测和评估)
bool PedestrianInteractionEvaluator::Evaluate(Obstacle* obstacle_ptr, ObstaclesContainer* obstacles_container) {
//检查健全性(初始化)
CHECK_NOTNULL(obstacle_ptr);
obstacle_ptr->SetEvaluatorType(evaluator_type_);
int id = obstacle_ptr->id();
if (!obstacle_ptr->latest_feature().IsInitialized()) {
AERROR << "Obstacle [" << id << "] has no latest feature.";
return false;
}
Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
CHECK_NOTNULL(latest_feature_ptr);
//提取障碍物的特征信息并根据不同模式处理
//-离线模式, 在本地保存以供训练
//-在线模式, 通过训练模型进行评估
std::vector<double> feature_values;
ExtractFeatures(obstacle_ptr, &feature_values);
if (FLAGS_prediction_offline_mode == PredictionConstants::kDumpDataForLearning) {
FeatureOutput::InsertDataForLearning(*latest_feature_ptr, feature_values, "pedestrian", nullptr);
ADEBUG << "Saving extracted features for learning locally.";
return true;
}
static constexpr double kShortTermPredictionTimeResolution = 0.4;
static constexpr int kShortTermPredictionPointNum = 5;
static constexpr int kHiddenStateUpdateCycle = 4;
//Step 1 Get social embedding
torch::Tensor social_pooling = GetSocialPooling();
std::vector<torch::jit::IValue> social_embedding_inputs;
social_embedding_inputs.push_back(std::move(social_pooling.to(device_)));
torch::Tensor social_embedding =
torch_social_embedding_.forward(social_embedding_inputs)
.toTensor()
.to(torch::kCPU);
// Step 2 Get position embedding
double pos_x = feature_values[2];
double pos_y = feature_values[3];
double rel_x = 0.0;
double rel_y = 0.0;
if (obstacle_ptr->history_size() > kHiddenStateUpdateCycle - 1) {
rel_x = obstacle_ptr->latest_feature().position().x() - obstacle_ptr->feature(3).position().x();
rel_y = obstacle_ptr->latest_feature().position().y() - obstacle_ptr->feature(3).position().y();
}
torch::Tensor torch_position = torch::zeros({1, 2});
torch_position[0][0] = rel_x;
torch_position[0][1] = rel_y;
std::vector<torch::jit::IValue> position_embedding_inputs;
position_embedding_inputs.push_back(std::move(torch_position.to(device_)));
torch::Tensor position_embedding = torch_position_embedding_.forward(position_embedding_inputs).toTensor().to(torch::kCPU);
// Step 3 Conduct single LSTM and update hidden states
torch::Tensor lstm_input =
torch::zeros({1, 2 * (kEmbeddingSize + kHiddenSize)});
for (int i = 0; i < kEmbeddingSize; ++i) {
lstm_input[0][i] = position_embedding[0][i];
}
if (obstacle_id_lstm_state_map_.find(id) == obstacle_id_lstm_state_map_.end()) {
obstacle_id_lstm_state_map_[id].ht = torch::zeros({1, 1, kHiddenSize});
obstacle_id_lstm_state_map_[id].ct = torch::zeros({1, 1, kHiddenSize});
obstacle_id_lstm_state_map_[id].timestamp = obstacle_ptr->timestamp();
obstacle_id_lstm_state_map_[id].frame_count = 0;
}
torch::Tensor curr_ht = obstacle_id_lstm_state_map_[id].ht;
torch::Tensor curr_ct = obstacle_id_lstm_state_map_[id].ct;
int curr_frame_count = obstacle_id_lstm_state_map_[id].frame_count;
if (curr_frame_count == kHiddenStateUpdateCycle - 1) {
for (int i = 0; i < kHiddenSize; ++i) {
lstm_input[0][kEmbeddingSize + i] = curr_ht[0][0][i];
lstm_input[0][kEmbeddingSize + kHiddenSize + i] = curr_ct[0][0][i];
}
std::vector<torch::jit::IValue> lstm_inputs;
lstm_inputs.push_back(std::move(lstm_input.to(device_)));
auto lstm_out_tuple = torch_single_lstm_.forward(lstm_inputs).toTuple();
auto ht = lstm_out_tuple->elements()[0].toTensor();
auto ct = lstm_out_tuple->elements()[1].toTensor();
obstacle_id_lstm_state_map_[id].ht = ht.clone();
obstacle_id_lstm_state_map_[id].ct = ct.clone();
}
obstacle_id_lstm_state_map_[id].frame_count = (curr_frame_count + 1) % kHiddenStateUpdateCycle;
//Step 4 for-loop get a trajectory
//Set the starting trajectory point
Trajectory* trajectory = latest_feature_ptr->add_predicted_trajectory();
trajectory->set_probability(1.0);
TrajectoryPoint* start_point = trajectory->add_trajectory_point();
start_point->mutable_path_point()->set_x(pos_x);
start_point->mutable_path_point()->set_y(pos_y);
start_point->mutable_path_point()->set_theta(latest_feature_ptr->theta());
start_point->set_v(latest_feature_ptr->speed());
start_point->set_relative_time(0.0);
for (int i = 1; i <= kShortTermPredictionPointNum; ++i) {
double prev_x = trajectory->trajectory_point(i - 1).path_point().x();
double prev_y = trajectory->trajectory_point(i - 1).path_point().y();
ACHECK(obstacle_id_lstm_state_map_.find(id) !=
obstacle_id_lstm_state_map_.end());
torch::Tensor torch_position = torch::zeros({1, 2});
double curr_rel_x = rel_x;
double curr_rel_y = rel_y;
if (i > 1) {
curr_rel_x =
prev_x - trajectory->trajectory_point(i - 2).path_point().x();
curr_rel_y =
prev_y - trajectory->trajectory_point(i - 2).path_point().y();
}
torch_position[0][0] = curr_rel_x;
torch_position[0][1] = curr_rel_y;
std::vector<torch::jit::IValue> position_embedding_inputs;
position_embedding_inputs.push_back(std::move(torch_position.to(device_)));
torch::Tensor position_embedding = torch_position_embedding_.forward(position_embedding_inputs).toTensor().to(torch::kCPU);
torch::Tensor lstm_input = torch::zeros({1, kEmbeddingSize + 2 * kHiddenSize});
for (int i = 0; i < kEmbeddingSize; ++i) {
lstm_input[0][i] = position_embedding[0][i];
}
auto ht = obstacle_id_lstm_state_map_[id].ht.clone();
auto ct = obstacle_id_lstm_state_map_[id].ct.clone();
for (int i = 0; i < kHiddenSize; ++i) {
lstm_input[0][kEmbeddingSize + i] = ht[0][0][i];
lstm_input[0][kEmbeddingSize + kHiddenSize + i] = ct[0][0][i];
}
std::vector<torch::jit::IValue> lstm_inputs;
lstm_inputs.push_back(std::move(lstm_input.to(device_)));
auto lstm_out_tuple = torch_single_lstm_.forward(lstm_inputs).toTuple();
ht = lstm_out_tuple->elements()[0].toTensor();
ct = lstm_out_tuple->elements()[1].toTensor();
std::vector<torch::jit::IValue> prediction_inputs;
prediction_inputs.push_back(ht[0]);
auto pred_out_tensor = torch_prediction_layer_.forward(prediction_inputs).toTensor().to(torch::kCPU);
auto pred_out = pred_out_tensor.accessor<float, 2>();
TrajectoryPoint* point = trajectory->add_trajectory_point();
double curr_x = prev_x + static_cast<double>(pred_out[0][0]);
double curr_y = prev_y + static_cast<double>(pred_out[0][1]);
point->mutable_path_point()->set_x(curr_x);
point->mutable_path_point()->set_y(curr_y);
point->set_v(latest_feature_ptr->speed());
point->mutable_path_point()->set_theta(latest_feature_ptr->velocity_heading());
point->set_relative_time(kShortTermPredictionTimeResolution * static_cast<double>(i));
}
return true;
}
ContainerSubmoduleProcess(处理各模块信息并更新发布)
bool PredictionComponent::ContainerSubmoduleProcess(const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
constexpr static size_t kHistorySize = 10;
const auto frame_start_time = Clock::Now();
//读取Localization信息，并且调用OnLocalization更新
localization_reader_->Observe();
auto ptr_localization_msg = localization_reader_->GetLatestObserved();
if (ptr_localization_msg == nullptr) {
AERROR << "Prediction: cannot receive any localization message.";
return false;
}
MessageProcess::OnLocalization(container_manager_.get(),*ptr_localization_msg);
//读取Planning上一时刻的信息
planning_reader_->Observe();
auto ptr_trajectory_msg = planning_reader_->GetLatestObserved();
if (ptr_trajectory_msg != nullptr) {
MessageProcess::OnPlanning(container_manager_.get(), *ptr_trajectory_msg);
}
//同上
storytelling_reader_->Observe();
auto ptr_storytelling_msg = storytelling_reader_->GetLatestObserved();
if (ptr_storytelling_msg != nullptr) {
MessageProcess::OnStoryTelling(container_manager_.get(),*ptr_storytelling_msg);
}
=====================================================================================================================
MessageProcess::ContainerProcess(container_manager_, *perception_obstacles,scenario_manager_.get());
auto obstacles_container_ptr = container_manager_->GetContainer<ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
CHECK_NOTNULL(obstacles_container_ptr);
auto adc_trajectory_container_ptr = container_manager_->GetContainer<ADCTrajectoryContainer>(AdapterConfig::PLANNING_TRAJECTORY);
CHECK_NOTNULL(adc_trajectory_container_ptr);
//获取子模块输出
SubmoduleOutput submodule_output = obstacles_container_ptr->GetSubmoduleOutput(kHistorySize,frame_start_time);
submodule_output.set_curr_scenario(scenario_manager_->scenario());
container_writer_->Write(submodule_output);
adc_container_writer_->Write(*adc_trajectory_container_ptr);
perception_obstacles_writer_->Write(*perception_obstacles);
return true;
}
PredictionEndToEndProc
bool PredictionComponent::PredictionEndToEndProc(const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
//测试模式
if (FLAGS_prediction_test_mode && (Clock::NowInSeconds() - component_start_time_ > FLAGS_prediction_test_duration)) {
ADEBUG << "Prediction finished running in test mode";
}
//检查相对地图(实时)
if (FLAGS_use_navigation_mode && !PredictionMap::Ready()) {
AERROR << "Relative map is empty.";
return false;
}
frame_start_time_ = Clock::NowInSeconds();
auto end_time1 = std::chrono::system_clock::now();
//读取Localization信息并更新
localization_reader_->Observe();
auto ptr_localization_msg = localization_reader_->GetLatestObserved();
if (ptr_localization_msg == nullptr) {
AERROR << "Prediction: cannot receive any localization message.";
return false;
}
MessageProcess::OnLocalization(container_manager_.get(),*ptr_localization_msg);
auto end_time2 = std::chrono::system_clock::now();
std::chrono::duration<double> diff = end_time2 - end_time1;
ADEBUG << "Time for updating PoseContainer: " << diff.count() * 1000 << " msec.";
//读取Storytelling消息并更新
storytelling_reader_->Observe();
auto ptr_storytelling_msg = storytelling_reader_->GetLatestObserved();
if (ptr_storytelling_msg != nullptr) {
MessageProcess::OnStoryTelling(container_manager_.get(),*ptr_storytelling_msg);
}
planning_reader_->Observe();
auto ptr_trajectory_msg = planning_reader_->GetLatestObserved();
if (ptr_trajectory_msg != nullptr) {
MessageProcess::OnPlanning(container_manager_.get(), *ptr_trajectory_msg);
}
auto end_time3 = std::chrono::system_clock::now();
diff = end_time3 - end_time2;
ADEBUG << "Time for updating ADCTrajectoryContainer: " << diff.count() * 1000 << " msec.";
=====================================================================================================================
//获取所有perception_obstacles，并调用OnPerception进行处理
auto perception_msg = *perception_obstacles;
PredictionObstacles prediction_obstacles;
//将Perception模块内容转换为Prediction模块可用的格式
MessageProcess::OnPerception(perception_msg, container_manager_, evaluator_manager_.get(),predictor_manager_.get(), scenario_manager_.get(), &prediction_obstacles);
auto end_time4 = std::chrono::system_clock::now();
diff = end_time4 - end_time3;
ADEBUG << "Time for updating PerceptionContainer: " << diff.count() * 1000 << " msec.";
//对预测的障碍物信息进行后续处理
prediction_obstacles.set_start_timestamp(frame_start_time_);
prediction_obstacles.set_end_timestamp(Clock::NowInSeconds());
prediction_obstacles.mutable_header()->set_lidar_timestamp(perception_msg.header().lidar_timestamp());
prediction_obstacles.mutable_header()->set_camera_timestamp(perception_msg.header().camera_timestamp());
prediction_obstacles.mutable_header()->set_radar_timestamp(perception_msg.header().radar_timestamp());
prediction_obstacles.set_perception_error_code(perception_msg.error_code());
//循环遍历所有trajectory_point，检查合法性(预测的轨迹可能存在问题)
if (FLAGS_prediction_test_mode) {
for (auto const& prediction_obstacle :
prediction_obstacles.prediction_obstacle()) {
for (auto const& trajectory : prediction_obstacle.trajectory()) {
for (auto const& trajectory_point : trajectory.trajectory_point()) {
if (!ValidationChecker::ValidTrajectoryPoint(trajectory_point)) {
AERROR << "Invalid trajectory point [" << trajectory_point.ShortDebugString() << "]";
break;
}
}
}
}
}
auto end_time5 = std::chrono::system_clock::now();
diff = end_time5 - end_time1;
ADEBUG << "End to end time elapsed: " << diff.count() * 1000 << " msec.";
//发布prediction_obstacles
common::util::FillHeader(node_->Name(), &prediction_obstacles);
prediction_writer_->Write(prediction_obstacles);
return true;
}
容器存储来自订阅通道的输入数据
评估器对任何给定的障碍分别预测路径和速度
预测器生成障碍物的预测轨迹
Routing模块
Routing模块主要关注起点到终点的长期路径，需要根据起点到终点之间的道路，选择一条最优路径
输入：地图数据(Map)和路由请求(routing_request)
输出：规划的路径
关联模块：Map
routing_component.cc
#include "modules/routing/routing_component.h"
#include <utility>
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/routing/common/routing_gflags.h"
DECLARE_string(flagfile);
namespace apollo {
namespace routing {
bool RoutingComponent::Init() {
//加载配置文件
RoutingConfig routing_conf;
ACHECK(cyber::ComponentBase::GetProtoConfig(&routing_conf))
<< "Unable to load routing conf file: "
<< cyber::ComponentBase::ConfigFilePath();
AINFO << "Config file: " << cyber::ComponentBase::ConfigFilePath() << " is loaded.";
//配置QOS，
apollo::cyber::proto::RoleAttributes attr;
attr.set_channel_name(routing_conf.topic_config().routing_response_topic());
auto qos = attr.mutable_qos_profile();
//保持最新历史
qos->set_history(apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
//要求可靠传输
qos->set_reliability(apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
//消息在本例持久化(断线依旧可用)
qos->set_durability(apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
//创建消息发布
response_writer_ = node_->CreateWriter<RoutingResponse>(attr);
apollo::cyber::proto::RoleAttributes attr_history;
attr_history.set_channel_name(routing_conf.topic_config().routing_response_history_topic());
auto qos_history = attr_history.mutable_qos_profile();
qos_history->set_history(apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
qos_history->set_reliability(apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
qos_history->set_durability(apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
response_history_writer_ = node_->CreateWriter<RoutingResponse>(attr_history);
//创建定时器，按间隔更新时间戳
std::weak_ptr<RoutingComponent> self = std::dynamic_pointer_cast<RoutingComponent>(shared_from_this());
timer_.reset(new ::apollo::cyber::Timer(
FLAGS_routing_response_history_interval_ms,
[self, this]() {
auto ptr = self.lock();
if (ptr) {
std::lock_guard<std::mutex> guard(this->mutex_);
if (this->response_ != nullptr) {
auto timestamp = apollo::cyber::Clock::NowInSeconds();
response_->mutable_header()->set_timestamp_sec(timestamp);
//记录历史
this->response_history_writer_->Write(*response_);
}
}
},
false));
timer_->Start();
//初始化并启动
1.return routing_.Init().ok() && routing_.Start().ok();
}
//收到路由请求时触发
bool RoutingComponent::Proc(const std::shared_ptr<RoutingRequest>& request) {
auto response = std::make_shared<RoutingResponse>();
//处理Routing请求
2.if (!routing_.Process(request, response.get())) {
return false;
}
//填充头部并发布响应(协作)
common::util::FillHeader(node_->Name(), response.get());
response_writer_->Write(response);
{
std::lock_guard<std::mutex> guard(mutex_);
response_ = std::move(response);
}
return true;
}
}
}
routing.cc
#include "modules/routing/routing.h"
#include <limits>
#include <unordered_map>
#include "modules/common/util/point_factory.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/routing/common/routing_gflags.h"
namespace apollo {
namespace routing {
using apollo::common::ErrorCode;
using apollo::common::PointENU;
using apollo::hdmap::ParkingSpaceInfoConstPtr;
apollo::common::Status Routing::Init() {
//读取地图文件(RoutingMap主要由点和边组成)
const auto routing_map_file = apollo::hdmap::RoutingMapFile();
AINFO << "Use routing topology graph path: " << routing_map_file;
navigator_ptr_.reset(new Navigator(routing_map_file));
//加载BaseMap
hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
ACHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();
return apollo::common::Status::OK();
}
apollo::common::Status Routing::Start() {
if (!navigator_ptr_->IsReady()) {
AERROR << "Navigator is not ready!";
return apollo::common::Status(ErrorCode::ROUTING_ERROR,"Navigator not ready");
}
//发布Routing状态
AINFO << "Routing service is ready.";
monitor_logger_buffer_.INFO("Routing started");
return apollo::common::Status::OK();
}
bool Routing::Process(const std::shared_ptr<RoutingRequest>& routing_request,RoutingResponse* const routing_response) {
CHECK_NOTNULL(routing_response);
AINFO << "Get new routing request:" << routing_request->DebugString();
//对车道信息进行填充(寻找最近车道)
1.const auto& fixed_requests = FillLaneInfoIfMissing(*routing_request);
double min_routing_length = std::numeric_limits<double>::max();
//从所有额外路径中挑选最优的
for (const auto& fixed_request : fixed_requests) {
RoutingResponse routing_response_temp;
if (navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)) {
const double routing_length = routing_response_temp.measurement().distance();
if (routing_length < min_routing_length) {
routing_response->CopyFrom(routing_response_temp);
min_routing_length = routing_length;
}
}
//对停车信息进行填充
FillParkingID(routing_response);
}
//寻找到合适路径，停车位置规划完毕
if (min_routing_length < std::numeric_limits<double>::max() && SupplementParkingRequest(routing_response)) {
monitor_logger_buffer_.INFO("Routing success!");
return true;
}
AERROR << "Failed to search route with navigator.";
monitor_logger_buffer_.WARN("Routing failed! " + routing_response->status().msg());
return false;
}
}
}
FillLaneInfoIfMissing(寻找距离请求点最近的车道)
std::vector<RoutingRequest> Routing::FillLaneInfoIfMissing(const RoutingRequest& routing_request) {
std::vector<RoutingRequest> fixed_requests;
std::unordered_map<int, std::vector<LaneWaypoint>>additional_lane_waypoint_map;
RoutingRequest fixed_request(routing_request);
for (int i = 0; i < routing_request.waypoint_size(); ++i) {
LaneWaypoint lane_waypoint(routing_request.waypoint(i));
if (lane_waypoint.has_id()) {
continue;
}
//填充缺少的车道信息
const auto point =common::util::PointFactory::ToPointENU(lane_waypoint.pose());//将pose信息转换为坐标点
std::vector<std::shared_ptr<const hdmap::LaneInfo>> lanes;
//在没有找到车道时扩大半径，寻找最近的车道
constexpr double kRadius = 0.3;
for (int i = 0; i < 20; ++i) {
hdmap_->GetLanes(point, kRadius + i * kRadius, &lanes);
if (lanes.size() > 0) {
break;
}
}
if (lanes.empty()) {
AERROR << "Failed to find nearest lane from map at position: " << point.DebugString();
return fixed_requests;//返回空的Vector
}
for (size_t j = 0; j < lanes.size(); ++j) {
double s = 0.0;
double l = 0.0;
lanes[j]->GetProjection({point.x(), point.y()}, &s, &l);
if (j == 0) {
auto waypoint_info = fixed_request.mutable_waypoint(i);
waypoint_info->set_id(lanes[j]->id().id());
waypoint_info->set_s(s);
} else {
//额外的备选车道
LaneWaypoint new_lane_waypoint(lane_waypoint);
new_lane_waypoint.set_id(lanes[j]->id().id());
new_lane_waypoint.set_s(s);
additional_lane_waypoint_map[i].push_back(new_lane_waypoint);
}
}
}
//第一个处理后的路由请求
fixed_requests.push_back(fixed_request);
//由于车道重叠生成额外路由请求
for (const auto& m : additional_lane_waypoint_map) {
size_t cur_size = fixed_requests.size();
for (size_t i = 0; i < cur_size; ++i) {
//使用index迭代的同时push_back
for (const auto& lane_waypoint : m.second) {
RoutingRequest new_request(fixed_requests[i]);
auto waypoint_info = new_request.mutable_waypoint(m.first);
waypoint_info->set_id(lane_waypoint.id());
waypoint_info->set_s(lane_waypoint.s());
fixed_requests.push_back(new_request);
}
}
}//在主要路由请求的基础上，将备选车道转换为额外路由请求，应对车道重叠的情况(为路径规划器提供更多选择)
for (const auto& fixed_request : fixed_requests) {
ADEBUG << "Fixed routing request:" << fixed_request.DebugString();
}
return fixed_requests;
}
Canbus模块
Canbus模块可以利用订阅的信息，向车辆控制系统传递命令以达到控制底盘(Chassis)的目的，同时可以实时进行车辆数据的接受与发送
输入：控制命令(Control Command)+保护命令(Guardian Command)
输出：汽车底盘信息(Chassis)
关联模块：Control+Guardian
canbus_component.cc
void CanbusComponent::Clear() {
vehicle_object_->Stop();
AINFO << "Cleanup Canbus component";
}
bool CanbusComponent::Init() {
//加载配置文件
if (!GetProtoConfig(&canbus_conf_)) {
AERROR << "Unable to load canbus conf file: " << ConfigFilePath();
return false;
}
AINFO << "The canbus conf file is loaded: " << FLAGS_canbus_conf_file;
ADEBUG << "Canbus_conf:" << canbus_conf_.ShortDebugString();
//加载车辆工厂库
if (!apollo::cyber::common::PathExists(FLAGS_load_vehicle_library)) {
AERROR << FLAGS_load_vehicle_library << " No such vehicle library";
return false;
}
AINFO << "Load the vehicle factory library: " << FLAGS_load_vehicle_library;
//创建车辆对象
ClassLoader loader(FLAGS_load_vehicle_library);
auto vehicle_object = loader.CreateClassObj<AbstractVehicleFactory>(FLAGS_load_vehicle_class_name);
if (!vehicle_object) {
AERROR << "Failed to create the vehicle factory: " << FLAGS_load_vehicle_class_name;
return false;
}
vehicle_object_ = vehicle_object;
if (vehicle_object_ == nullptr) {
AERROR << "Failed to create vehicle factory pointer.";
}
AINFO << "Successfully create vehicle factory: " << FLAGS_load_vehicle_class_name;
//初始化车辆对象并传入配置信息
if (!vehicle_object_->Init(&canbus_conf_)) {
AERROR << "Fail to init vehicle factory.";
return false;
}
AINFO << "Vehicle factory is successfully initialized.";
//配置消息订阅器(guardian+control)
cyber::ReaderConfig guardian_cmd_reader_config;
guardian_cmd_reader_config.channel_name = FLAGS_guardian_topic;
guardian_cmd_reader_config.pending_queue_size = FLAGS_guardian_cmd_pending_queue_size;
cyber::ReaderConfig control_cmd_reader_config;
control_cmd_reader_config.channel_name = FLAGS_control_command_topic;
control_cmd_reader_config.pending_queue_size = FLAGS_control_cmd_pending_queue_size;
if (FLAGS_receive_guardian) {
guardian_cmd_reader_ = node_->CreateReader<GuardianCommand>(
guardian_cmd_reader_config,[this](const std::shared_ptr<GuardianCommand> &cmd) {
ADEBUG << "Received guardian data: run canbus callback.";
OnGuardianCommand(*cmd);
});
} else {
control_command_reader_ = node_->CreateReader<ControlCommand>(
control_cmd_reader_config,[this](const std::shared_ptr<ControlCommand> &cmd) {
ADEBUG << "Received control data: run canbus callback.";
1.OnControlCommand(*cmd);
});
}
//创建Chassis写入器
chassis_writer_ = node_->CreateWriter<Chassis>(FLAGS_chassis_topic);
//启动Canbus相关组件
if (!vehicle_object_->Start()) {
AERROR << "Fail to start canclient, cansender, canreceiver, canclient, "
"vehicle controller.";//缺少判断？
}
AINFO << "Start canclient cansender, canreceiver, canclient, vehicle "
"controller successfully.";
monitor_logger_buffer_.INFO("Canbus is started.");
return true;
}
bool CanbusComponent::Proc() {
2.PublishChassis();
if (FLAGS_enable_chassis_detail_pub) {
vehicle_object_->PublishChassisDetail();
}
//表明正在运行
vehicle_object_->UpdateHeartbeat();
return true;
}
common::Status CanbusComponent::OnError(const std::string &error_msg) {
monitor_logger_buffer_.ERROR(error_msg);
return ::apollo::common::Status(ErrorCode::CANBUS_ERROR, error_msg);
}
OnControlCommand(执行车辆控制命令)
void CanbusComponent::OnControlCommand(const ControlCommand &control_command) {
int64_t current_timestamp = Time::Now().ToMicrosecond();
//如果命令的到达速度超过了系统的处理能力，可以选择忽略
if (current_timestamp - last_timestamp_ < FLAGS_min_cmd_interval * 1000) {
ADEBUG << "Control command comes too soon. Ignore.\n Required "
"FLAGS_min_cmd_interval["
<< FLAGS_min_cmd_interval << "], actual time interval[" << current_timestamp - last_timestamp_ << "].";
return;
}
//记录新的时间戳
last_timestamp_ = current_timestamp;
//计算延迟
ADEBUG << "Control_sequence_number:" << control_command.header().sequence_num() << ", Time_of_delay:"
<< current_timestamp - static_cast<int64_t>(control_command.header().timestamp_sec() *1e6) << " micro seconds";
//将命令传递给底层的车辆控制系统
vehicle_object_->UpdateCommand(&control_command);
}
PublishChassis(发布底盘信息)
void CanbusComponent::PublishChassis() {
Chassis chassis = vehicle_object_->publish_chassis();
//填充头部信息
common::util::FillHeader(node_->Name(), &chassis);
chassis_writer_->Write(chassis);
ADEBUG << chassis.ShortDebugString();
}
Guardian模块
Guardian模块通过监测车辆的状态来识别潜在的风险，采取适当的措施来保障车辆运行的安全
输入：底盘(Chassis)，control_cmd以及system_status信息
输出：保护指令(guardian_cmd_)
关联模块：Canbus+Control
guardian_component.cc
bool GuardianComponent::Init() {
if (!GetProtoConfig(&guardian_conf_)) {
AERROR << "Unable to load canbus conf file: " << ConfigFilePath();
return false;
}
//订阅chassis，control_cmd以及system_status信息
chassis_reader_ = node_->CreateReader<Chassis>(FLAGS_chassis_topic, [this](const std::shared_ptr<Chassis>& chassis) {
ADEBUG << "Received chassis data: run chassis callback.";
std::lock_guard<std::mutex> lock(mutex_);
chassis_.CopyFrom(*chassis);
});
control_cmd_reader_ = node_->CreateReader<ControlCommand>(FLAGS_control_command_topic,[this](const std::shared_ptr<ControlCommand>& cmd) {
ADEBUG << "Received control data: run control callback.";
std::lock_guard<std::mutex> lock(mutex_);
control_cmd_.CopyFrom(*cmd);
});
system_status_reader_ = node_->CreateReader<SystemStatus>(FLAGS_system_status_topic,[this](const std::shared_ptr<SystemStatus>& status) {
ADEBUG << "Received system status data: run system status callback.";
std::lock_guard<std::mutex> lock(mutex_);
last_status_received_s_ = Time::Now().ToSecond();
system_status_.CopyFrom(*status);
});
//guardian消息发布
guardian_writer_ = node_->CreateWriter<GuardianCommand>(FLAGS_guardian_topic);
return true;
}
bool GuardianComponent::Proc() {
constexpr double kSecondsTillTimeout(2.5);
bool safety_mode_triggered = false;
//满足条件时进入安全模式
if (guardian_conf_.guardian_enable()) {
std::lock_guard<std::mutex> lock(mutex_);
if (Time::Now().ToSecond() - last_status_received_s_ > kSecondsTillTimeout) {
safety_mode_triggered = true;
}
safety_mode_triggered = safety_mode_triggered || system_status_.has_safety_mode_trigger_time();
}
//依据状态触发安全模式或继续传递控制命令
if (safety_mode_triggered) {
ADEBUG << "Safety mode triggered, enable safety mode";
1.TriggerSafetyMode();
} else {
ADEBUG << "Safety mode not triggered, bypass control command";
PassThroughControlCommand();
}
//填充头部信息并发布命令
common::util::FillHeader(node_->Name(), &guardian_cmd_);
guardian_writer_->Write(guardian_cmd_);
return true;
}
TriggerSafetyMode(触发安全模式)
void GuardianComponent::TriggerSafetyMode() {
AINFO << "Safety state triggered, with system safety mode trigger time : " << system_status_.safety_mode_trigger_time();
std::lock_guard<std::mutex> lock(mutex_);
bool sensor_malfunction = false, obstacle_detected = false;
//检查超声波传感器
if (!chassis_.surround().sonar_enabled() || chassis_.surround().sonar_fault()) {
AINFO << "Ultrasonic sensor not enabled for faulted, will do emergency stop!";
sensor_malfunction = true;
} else {
//当障碍物过近或传感器故障
for (int i = 0; i < chassis_.surround().sonar_range_size(); ++i) {
if ((chassis_.surround().sonar_range(i) > 0.0 &&
chassis_.surround().sonar_range(i) < 2.5) ||
chassis_.surround().sonar_range(i) > 30) {
AINFO << "Object detected or ultrasonic sensor fault output, will do emergency stop!";
obstacle_detected = true;
}
}
}
//设置安全模式下控制命令
guardian_cmd_.mutable_control_command()->set_throttle(0.0);
guardian_cmd_.mutable_control_command()->set_steering_target(0.0);
guardian_cmd_.mutable_control_command()->set_steering_rate(25.0);
guardian_cmd_.mutable_control_command()->set_is_in_safe_mode(true);
//***TODO(QiL) : Remove this one once hardware re-alignment is done.(重新校准)
sensor_malfunction = false;
obstacle_detected = false;
AINFO << "Temporarily ignore the ultrasonic sensor output during hardware re-alignment!";
//根据情况采取制动
if (system_status_.require_emergency_stop() || sensor_malfunction || obstacle_detected) {
AINFO << "Emergency stop triggered! with system status from monitor as : "
<< system_status_.require_emergency_stop();
guardian_cmd_.mutable_control_command()->set_brake(guardian_conf_.guardian_cmd_emergency_stop_percentage());
} else {
AINFO << "Soft stop triggered! with system status from monitor as : "
<< system_status_.require_emergency_stop();
guardian_cmd_.mutable_control_command()->set_brake(guardian_conf_.guardian_cmd_soft_stop_percentage());
}
}
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
