源碼位於 /modules/drivers/radar/conti_radar
引用官方
主要文件
- main.cc
- apollo_app.cc
- apollo_app.h
mian.c文件
文件中只有一行代碼:APOLLO_MAIN(ContiRadarCanbus) ,使用宏APOLLO_MAIN,開啟了 ContiRadarCanbus 節點,這里 ContiRadarCanbus 節點開始運行,這里的的節點與ros中的node概念一致,相當於一個進程
APOLLO_MAIN宏解析
APOLLO_MAIN 宏定義位於"modules/common/apollo_app.h"文件。
設置log和SIGINT信號處理程序,收到信號,關閉本節點。
創建模塊類對象,設置節點名字,調用基類(ApolloApp)的Spin()函數。
ApolloApp類:
Spin()函數屬於類ApolloApp public成員函數,類ApolloApp是所有模塊類的基類。
Public成員有name()函數,用於獲取模塊名字。Spin()函數用於初始化、啟動、當ros關閉時關閉模塊節點。還有一個析構函數。
Protected成員都是vritual接口,子類都會重寫,在Spin()函數中調用,其實現實在具體各個模塊內部。Init()函數完成加載模塊的配置文件,創建訂閱話題。Start()函數:注冊回調函數,回調函數負責節點核心任務,通常由上游話題或者timer觸發。Stop()函數,結束節點,正常時不會執行到。ReportModuleStatus()返回模塊狀態。apollo_app_sigint_handler()函數,信號處理函數。
Spin()函數使用init(),start()和stop()函數完成模塊節點的實現。此函數一般不會被重寫,也就是使用ApolloApp的實現。
ContiRadarCanbus
ContiRadarCanbus 繼承自 ApolloApp,實現了 Init Start Stop 等方法 ,當 ApolloApp 啟動后會執行 Spin 函數來 初始化啟動節點以及 ros stop 的監聽。
-
int ApolloApp::Spin() {
-
// 調用子類的 Init,完成配置文件的加載
-
auto status = Init();
-
if (!status.ok()) {
-
AERROR << Name() << " Init failed: " << status;
-
return -1;
-
}
-
...
-
// 調用子類的 Start
-
status = Start();
-
if (!status.ok()) {
-
AERROR << Name() << " Start failed: " << status;
-
return -2;
-
}
-
...
-
// 當 Ros 關閉時,調用子類 Stop
-
ros::waitForShutdown();
-
Stop();
-
AINFO << Name() << " exited.";
-
return 0;
-
}
ContiRadarCanbus::Init 方法
在 Init 方法中,ContiRadarCanbus 先檢查了配置文件是否存在,通過配置文件創建了 CanClient 和 ContiRadarMessageManager 對象,並使用這兩個對象初始化了 CanReceiver 對象。CanClient 負責與 CAN 卡通訊,獲取消息或發送消息,ContiRadarMessageManager 則用來解析消息。而 CanReceiver 則調度這兩個對象的工作。
調度 CanClient 和 ContiRadarMessageManager 工作的代碼在 CanReceiver 的 RecvThreadFunc 方法中
-
template <typename SensorType>
-
void CanReceiver<SensorType>::RecvThreadFunc() {
-
...
-
while (IsRunning()) {
-
std::vector<CanFrame> buf;
-
int32_t frame_num = MAX_CAN_RECV_FRAME_LEN;
-
// 從 CanClient 中讀取數據
-
if (can_client_->Receive(&buf, &frame_num) !=
-
::apollo::common::ErrorCode::OK) {
-
...
-
}
-
...
-
for (const auto &frame : buf) {
-
uint8_t len = frame.len;
-
uint32_t uid = frame.id;
-
const uint8_t *data = frame.data;
-
// 調用 ContiRadarMessageManager 的 Parse 方法解析雷達數據
-
pt_manager_->Parse(uid, data, len);
-
...
-
}
-
}
-
}
這個方法會在 ContiRadarCanbus::Start 方法被調用的時候調用。
ContiRadarCanbus::Start 方法
Start 先調用了 CanClient 的 Start 方法啟動了雷達設備,之后又發送了配置信息去配置雷達設備,在確定消息已發送后又調用了 CanReceiver 的 Start 方法,之前講的 CanReceiver:RecvThreadFunc 方法,就在CanReceiver 的 Start 中被調用。
於是,CanReceiver:RecvThreadFunc 方法中不斷的取 CanClient 的消息,不斷的把消息給 ContiRadarMessageManager 解析。
ContiRadarMessageManager
ContiRadarMessageManager 繼承自 ::apollo::drivers::canbus::MessageManager,繼承 MessageManager 需要一個泛型參數來定義處理的數據實體。這里給的參數是 ContiRadar,ContiRadar 是類似 JavaBean 一樣的存在,由 Google Protocol Buffer 編譯生成。
ContiRadarMessageManager 的構造函數中添加了多個數據處理類的對象到 protocol_data_map_ 中,protocol_data_map_ 是一個集合,以ID為鍵,消息處理對象為值。
在 CanReceiver 的 RecvThreadFunc 方法中,CanReveiver 把 CanClient 獲取的數據,交給 ContiRadarMessageManager 的 Parse 方法處理。所以 Parse 是 ContiRadarMessageManager 中最重要的方法。
ContiRadarMessageManager::Parse
-
void Parse(const uint32_t message_id, const uint8_t *data, int32_t length)
-
Parse 方法接收 3 個參數,消息ID,數據,數據長度。
拿到數據后,Parse 方法先通過消息 ID 從 protocol_data_map_ 中獲取了對應的數據處理對象。該方法並不會拿到數據后直接開始解析,而是需要判斷雷達的狀態信息,如果與用戶匹配才開始解析並發送消息。
-
// 如果雷達未配置,且消息 ID 不是 201 的話就結束 Parse 方法
-
if (!is_configured_ && message_id != RadarState201::ID) {
-
// read radar state message first
-
return;
-
}
-
// 如果消息 ID 是 600 或 60A 的時候則觸發發送
-
if (message_id == ClusterListStatus600::ID ||
-
message_id == ObjectListStatus60A::ID) {
-
ADEBUG << sensor_data_.ShortDebugString();
-
-
if (sensor_data_.contiobs_size() <=
-
sensor_data_.object_list_status().nof_objects()) {
-
// maybe lost a object_list_status msg
-
AdapterManager::PublishContiRadar(sensor_data_);
-
}
-
sensor_data_.Clear();
-
// fill header when recieve the general info message
-
AdapterManager::FillContiRadarHeader(FLAGS_sensor_node_name, &sensor_data_);
-
}
-
-
// 解析數據
-
sensor_protocol_data->Parse(data, length, &sensor_data_);
-
-
// 否則消息是 201 ,則對比配置信息。
-
if (message_id == RadarState201::ID) {
-
ADEBUG << sensor_data_.ShortDebugString();
-
if (sensor_data_.radar_state().send_quality() ==
-
radar_config_.radar_conf().send_quality() &&
-
sensor_data_.radar_state().send_ext_info() ==
-
radar_config_.radar_conf().send_ext_info() &&
-
sensor_data_.radar_state().max_distance() ==
-
radar_config_.radar_conf().max_distance() &&
-
sensor_data_.radar_state().output_type() ==
-
radar_config_.radar_conf().output_type() &&
-
sensor_data_.radar_state().rcs_threshold() ==
-
radar_config_.radar_conf().rcs_threshold() &&
-
sensor_data_.radar_state().radar_power() ==
-
radar_config_.radar_conf().radar_power()) {
-
// 信息一致,雷達狀態已與用戶設置匹配
-
is_configured_ = true;
-
} else {
-
// 雷達狀態不匹配,再次配置雷達
-
AINFO << "configure radar again";
-
SenderMessage<ContiRadar> sender_message(RadarConfig200::ID,
-
&radar_config_);
-
sender_message.Update();
-
can_client_->SendSingleFrame({sender_message.CanFrame()});
-
}
-
}
我們發現有兩種特殊的消息 Id 需要處理。
一種是當消息 ID 為 201 的時候,201 是雷達的狀態信息,當拿到的是狀態信息的時候,需要判斷雷達狀態與用戶配置,如果不匹配則需要發送指令配置雷達。
第二種是消息 ID 為 600 或 60A,當消息 ID 為這兩種的時候,觸發消息的發布。