Apollo学习笔记(一):canbus模块与车辆底盘之间的CAN数据传输过程
博主现在从车载自组网信道分配和多跳路由转向了自动驾驶,没啥经验,想快些做出来个Demo还是得站在巨人的肩膀上才行,我选择了Apollo,主要还是支持国产而且它的开发者套件有现成的底盘可以直接跑起来,但是apollo系统结构比较复杂,各种花哨的设计模式(消息适配器、工厂模式等)绕得人头晕。日本那里有个autoware是基于原生ROS的,也用Apollo开发者套件跑了下,就是普通的机器人开发那套,难度适合学生用来做项目,后面继续深入研究一下。(注意代码里面是有我写的注释的)
这次的学习是基于Apollo3.0的,因为3.0还是基于ROS的,后期研究autoware开发自己的系统能用得上,而且那个开发者套件也要用3.0。
canbus模块启动过程
参考知行合一2018大佬的Apollo Planning模块源代码分析可以知道canbus模块的主入口为modules/canbus/main.cc
:
APOLLO_MAIN(apollo::canbus::Canbus);
该宏展开后为:
#define APOLLO_MAIN(APP) \
int main(int argc, char **argv) { \
google::InitGoogleLogging(argv[0]); \
google::ParseCommandLineFlags(&argc, &argv, true); \
signal(SIGINT, apollo::common::apollo_app_sigint_handler); \
APP apollo_app_; \
ros::init(argc, argv, apollo_app_.Name()); \
apollo_app_.Spin(); \
return 0; \
}
在apollo_app_.Spin();
函数内会依次调用modules/canbus/canbus.cc
的Init()
和Start()
。
Init()主要过程源码分析
Status Canbus::Init() {
AdapterManager::Init(FLAGS_canbus_adapter_config_filename);/*完成AdapterManager的初始化,
FLAGS_canbus_adapter_config_filename对应于modules/canbus/common/canbus_gflags.cc中的
DEFINE_string(canbus_adapter_config_filename, "modules/canbus/conf/adapter.conf",
"The adapter config file");
adapter.conf中配置了canbus模块订阅和发布的topic
如果改成原生ROS的话,这里的AdapterManager配置删掉,改成ROS的topic订阅和发布*/
AINFO << "The adapter manager is successfully initialized.";
// load conf
//导入配置文件modules/canbus/conf/canbus_conf.pb.txt
/*
vehicle_parameter {
brand: LINCOLN_MKZ//指定车辆,后面生成对应的vehicle_factory,进而生成对应的message_manager_
max_enable_fail_attempt: 5
driving_mode: COMPLETE_AUTO_DRIVE
}
can_card_parameter {
brand: ESD_CAN//指定车辆,后面生成对应的can_client_
type: PCI_CARD
channel_id: CHANNEL_ID_ZERO
}
enable_debug_mode: false
enable_receiver_log: false
enable_sender_log: false
*/
//如果改成原生ROS的话此处也可以删除,工厂模式也可以放一边,上面导入的配置文件就是用来生成具体的产品工厂
//对象和产品对象,我们直接用ROS跑自己的工程一般车辆和CAN卡是固定的,可以改动后直接生成对应的产品对象
if (!common::util::GetProtoFromFile(FLAGS_canbus_conf_file, &canbus_conf_)) {
return OnError("Unable to load canbus conf file: " +
FLAGS_canbus_conf_file);
}
AINFO << "The canbus conf file is loaded: " << FLAGS_canbus_conf_file;
ADEBUG << "Canbus_conf:" << canbus_conf_.ShortDebugString();
// Init can client
auto *can_factory = CanClientFactory::instance();
can_factory->RegisterCanClients();
can_client_ = can_factory->CreateCANClient(canbus_conf_.can_card_parameter());
/*
std::unique_ptr<CanClient> CanClientFactory::CreateCANClient(
const CANCardParameter& parameter) {
auto factory = CreateObject(parameter.brand());//这里确定了新建的can client对象,
//此处新建的是ESD CAN卡对应的can client对象(ESD CAN卡是开发者套件用的CAN卡),具体为
//modules/drivers/canbus/can_client/esd/esd_can_client.cc的对象。
//由此也可以看出modules/canbus/与modules/drivers/canbus/之间的联系是很紧密的,
//modules/canbus/偏向上层的针对不同车辆的针对性数据解析
//modules/drivers/canbus/偏向底层的一些共性功能的实现,比如
//modules/drivers/canbus/can_comm/message_manager就是
//modules/canbus/vehicle/lincoln/lincoln_message_manager的父类
//改成原生ROS时这两个文件夹可以合到一起
if (!factory) {
AERROR << "Failed to create CAN client with parameter: "
<< parameter.DebugString();
} else if (!factory->Init(parameter)) {
AERROR << "Failed to initialize CAN card with parameter: "
<< parameter.DebugString();
}
return factory;
}
*/
if (!can_client_) {
return OnError("Failed to create can client.");
}
AINFO << "Can client is successfully created.";
VehicleFactory vehicle_factory;
vehicle_factory.RegisterVehicleFactory();
auto vehicle_object =
vehicle_factory.CreateVehicle(canbus_conf_.vehicle_parameter());//类似地生成具体的车辆对象
if (!vehicle_object) {
return OnError("Failed to create vehicle:");
}
message_manager_ = vehicle_object->CreateMessageManager();//生成对应车辆的message_manager_
if (message_manager_ == nullptr) {
return OnError("Failed to create message manager.");
}
AINFO << "Message manager is successfully created.";
//初始化can_receiver_,就是将can_client_、message_manager_赋给can_receiver_的成员变量
if (can_receiver_.Init(can_client_.get(), message_manager_.get(),
canbus_conf_.enable_receiver_log()) != ErrorCode::OK) {
return OnError("Failed to init can receiver.");
}
AINFO << "The can receiver is successfully initialized.";
//初始化can_sender_,就是将can_client_赋给can_sender_的成员变量
if (can_sender_.Init(can_client_.get(), canbus_conf_.enable_sender_log()) !=
ErrorCode::OK) {
return OnError("Failed to init can sender.");
}
AINFO << "The can sender is successfully initialized.";
//生成对应车辆的vehicle_controller_
//至此vehicle_object生成了message_manager_和vehicle_controller_
//message_manager_ 用来解析canbus从CAN接收到的消息
//vehicle_controller_ 用来更新canbus发往CAN的数据对象,也叫作协议类型数据
//(如刹车这个命令(协议)对应的对象,apollo将每种命令建了个类,比如林肯的刹车对应的类是
//modules/canbus/vehicle/lincoln/protocol/brake_60.h),
//也就是canbus接收到上层control的commond后通过vehicle_controller_
//更新协议类型数据内的具体成员变量(如刹车这个命令对应的对象内的“刹车量pedal_cmd_”这一成员变量)
vehicle_controller_ = vehicle_object->CreateVehicleController();
if (vehicle_controller_ == nullptr) {
return OnError("Failed to create vehicle controller.");
}
AINFO << "The vehicle controller is successfully created.";
/*vehicle_controller_->Init(...) (实际上是
modules/canbus/vehicle/lincoln/lincoln_controller.cc中的LincolnController::Init)里面
先生成brake_60_等协议类型数据,接着通过
can_sender_->AddMessage(Brake60::ID, brake_60_, false);等将brake_60_等协议类型数据
加入can_sender_的成员变量send_messages_(向量),后面can_sender_发送数据时就会将send_messages_
中的各个协议类型数据包含的CAN帧can_frame_to_send_发送到底盘
*/
if (vehicle_controller_->Init(canbus_conf_.vehicle_parameter(), &can_sender_,
message_manager_.get()) != ErrorCode::OK) {
return OnError("Failed to init vehicle controller.");
}
AINFO << "The vehicle controller is successfully initialized.";
CHECK(AdapterManager::GetControlCommand()) << "Control is not initialized.";
CHECK(AdapterManager::GetGuardian()) << "Guardian is not initialized.";
// TODO(QiL) : depreacte this
//类似原生ROS的添加回调函数,Apollo将topic的发布和订阅都集成到了AdapterManager进行统一管理
//Canbus::OnControlCommand就是canbus模块对上层cotrol_command的回调函数
if (!FLAGS_receive_guardian) {
AdapterManager::AddControlCommandCallback(&Canbus::OnControlCommand, this);
} else {
AdapterManager::AddGuardianCallback(&Canbus::OnGuardianCommand, this);
}
return Status::OK();
}
Init()主要过程流程图(为方便整合不严格按照代码里的顺序)
Start()主要过程源码分析
/*Init()生成的对象有can_client_ , vehicle_object , message_manager_,
vehicle_controller_ , can_receiver_ 和 can_sender_,
Start()里面就是调用can_client_, can_receiver_, can_sender_和vehicle_controller_的Start()将
它们的功能各自启动起来,比如can_client_ 的Start() (实际上是
modules/drivers/canbus/can_client/esd/esd_can_client.cc的Start())
就是调用third_party/can_card_library/esd_can/include/ntcan.h的内置函数canOpen()等设置端口等
以启动CAN卡,ntcan.h是买CAN卡的时候附带光盘里的文件,买了开发者套件装好CAN卡以后要把这个文件拷到
third_party/can_card_library/esd_can/include/下使用。因为ntcan.h是花钱买的所以就不把ntcan.h
的代码放上来了。
最后启动定时器循环运行Canbus::OnTimer,OnTimer这个函数就是发布底盘信息用的,发布以后
订阅底盘topic的上层模块就能接收到底盘信息。
*/
Status Canbus::Start() {
// 1. init and start the can card hardware
if (can_client_->Start() != ErrorCode::OK) {
return OnError("Failed to start can client");
}
AINFO << "Can client is started.";
// 2. start receive first then send
if (can_receiver_.Start() != ErrorCode::OK) {
return OnError("Failed to start can receiver.");
}
AINFO << "Can receiver is started.";
// 3. start send
if (can_sender_.Start() != ErrorCode::OK) {
return OnError("Failed to start can sender.");
}
// 4. start controller
if (vehicle_controller_->Start() == false) {
return OnError("Failed to start vehicle controller.");
}
// 5. set timer to triger publish info periodly
const double duration = 1.0 / FLAGS_chassis_freq;
timer_ = AdapterManager::CreateTimer(ros::Duration(duration),
&Canbus::OnTimer, this);
// last step: publish monitor messages
apollo::common::monitor::MonitorLogBuffer buffer(&monitor_logger_);
buffer.INFO("Canbus is started.");
return Status::OK();
}
Start()主要过程流程图
下面分canbus模块向底盘发送数据和canbus模块从底盘接收数据两部分进行深入分析。
canbus模块向底盘发送数据
主要过程
canbus模块向底盘发送数据的开端在canbus模块接收到上层control_command,因此Canbus::OnControlCommand是发送的开端。
void Canbus::OnControlCommand(const ControlCommand &control_command) {
int64_t current_timestamp =
apollo::common::time::AsInt64<common::time::micros>(Clock::Now());
// if command coming too soon, just ignore it.
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 - control_command.header().timestamp_sec();
/*从此处开始的vehicle_controller_->Update(control_command)和can_sender_.Update()是关键
正如之前所提到的,vehicle_controller_->Update(control_command)用来更新协议类型数据的
成员变量,接着can_sender_.Update()把更新之后的数据通过can_frame_to_update_
赋值给can_frame_to_send_,can_sender_在Start()是开启的发送线程将can_frame_to_send_
通过CAN卡附带驱动内的canWrite()函数注入底盘CAN网络
这两个Update()涉及的函数比较多,中间挺多跳转,就不放源码了,跳转的过程在后面流程图中给出,具体代码的
实现大家还是需要自己去看代码
*/
if (vehicle_controller_->Update(control_command) != ErrorCode::OK) {
AERROR << "Failed to process callback function OnControlCommand because "
"vehicle_controller_->Update error.";
return;
}
can_sender_.Update();
}
vehicle_controller_->Update流程图(以刹车对应的协议类型数据为例)
can_sender_.Update()流程图(以刹车对应的协议类型数据为例)
canbus模块从底盘接收数据
主要过程
在canbus Start()的时候,can_receiver_.Start()启动了从底盘接收数据的线程,线程内运行CanReceiver::RecvThreadFunc()
// 2. start receive first then send
if (can_receiver_.Start() != ErrorCode::OK) {
return OnError("Failed to start can receiver.");
}
AINFO << "Can receiver is started.";
template <typename SensorType>
::apollo::common::ErrorCode CanReceiver<SensorType>::Start() {
if (is_init_ == false) {
return ::apollo::common::ErrorCode::CANBUS_ERROR;
}
is_running_ = true;
thread_.reset(new std::thread([this] { RecvThreadFunc(); }));
if (thread_ == nullptr) {
AERROR << "Unable to create can client receiver thread.";
return ::apollo::common::ErrorCode::CANBUS_ERROR;
}
return ::apollo::common::ErrorCode::OK;
}
RecvThreadFunc()内循环运行can_client_->Receive(&buf, &frame_num)
template <typename SensorType>
void CanReceiver<SensorType>::RecvThreadFunc() {
AINFO << "Can client receiver thread starts.";
CHECK_NOTNULL(can_client_);
CHECK_NOTNULL(pt_manager_);
int32_t receive_error_count = 0;
int32_t receive_none_count = 0;
const int32_t ERROR_COUNT_MAX = 10;
std::chrono::duration<double, std::micro> default_period{10 * 1000};
while (IsRunning()) {
std::vector<CanFrame> buf;
int32_t frame_num = MAX_CAN_RECV_FRAME_LEN;
if (can_client_->Receive(&buf, &frame_num) !=
::apollo::common::ErrorCode::OK) {
/*
can_client_为modules/drivers/canbus/can_client/esd/esd_can_client.cc的实例化对象,
其Receive()函数调用了third_party/can_card_library/esd_can/include/ntcan.h的canRead函数从
CAN网络中读取数据并存入buf,
const int32_t ret = canRead(dev_handler_, recv_frames_, frame_num, nullptr);
buf的定义是std::vector<CanFrame> buf;
CanFrame在之前can_sender_.Update()流程图内有分析。
*/
LOG_IF_EVERY_N(ERROR, receive_error_count++ > ERROR_COUNT_MAX,
ERROR_COUNT_MAX)
<< "Received " << receive_error_count << " error messages.";
std::this_thread::sleep_for(default_period);
continue;
}
receive_error_count = 0;
if (buf.size() != static_cast<size_t>(frame_num)) {
AERROR_EVERY(100) << "Receiver buf size [" << buf.size()
<< "] does not match can_client returned length["
<< frame_num << "].";
}
if (frame_num == 0) {
LOG_IF_EVERY_N(ERROR, receive_none_count++ > ERROR_COUNT_MAX,
ERROR_COUNT_MAX)
<< "Received " << receive_none_count << " empty messages.";
std::this_thread::sleep_for(default_period);
continue;
}
receive_none_count = 0;
for (const auto &frame : buf) {
uint8_t len = frame.len;
uint32_t uid = frame.id;
const uint8_t *data = frame.data;
pt_manager_->Parse(uid, data, len);
/*
pt_manager_在Init时被赋值为message_manager_(modules/canbus/vehicle/lincoln
/lincoln_message_manager.cc的实例化对象),message_manager_用来解析从CAN网络获取的CanFrame
*/
if (enable_log_) {
ADEBUG << "recv_can_frame#" << frame.CanFrameString();
}
}
std::this_thread::yield();
}
AINFO << "Can client receiver thread stopped.";
}
接收CAN数据流程图