apollo 毫米波雷達代碼層面如何獲取雷達信號


源碼位於 /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 的監聽。

  1.  
    int ApolloApp::Spin() {
  2.  
    // 調用子類的 Init,完成配置文件的加載
  3.  
    auto status = Init();
  4.  
    if (!status.ok()) {
  5.  
    AERROR << Name() << " Init failed: " << status;
  6.  
    return -1;
  7.  
    }
  8.  
    ...
  9.  
    // 調用子類的 Start
  10.  
    status = Start();
  11.  
    if (!status.ok()) {
  12.  
    AERROR << Name() << " Start failed: " << status;
  13.  
    return -2;
  14.  
    }
  15.  
    ...
  16.  
    // 當 Ros 關閉時,調用子類 Stop
  17.  
    ros::waitForShutdown();
  18.  
    Stop();
  19.  
    AINFO << Name() << " exited.";
  20.  
    return 0;
  21.  
    }

ContiRadarCanbus::Init 方法

在 Init 方法中,ContiRadarCanbus 先檢查了配置文件是否存在,通過配置文件創建了 CanClient 和 ContiRadarMessageManager 對象,並使用這兩個對象初始化了 CanReceiver 對象。CanClient 負責與 CAN 卡通訊,獲取消息或發送消息,ContiRadarMessageManager 則用來解析消息。而 CanReceiver 則調度這兩個對象的工作。
調度 CanClient 和 ContiRadarMessageManager 工作的代碼在 CanReceiver 的 RecvThreadFunc 方法中

  1.  
    template <typename SensorType>
  2.  
    void CanReceiver<SensorType>::RecvThreadFunc() {
  3.  
    ...
  4.  
    while (IsRunning()) {
  5.  
    std::vector<CanFrame> buf;
  6.  
    int32_t frame_num = MAX_CAN_RECV_FRAME_LEN;
  7.  
    // 從 CanClient 中讀取數據
  8.  
    if (can_client_->Receive(&buf, &frame_num) !=
  9.  
    ::apollo::common::ErrorCode::OK) {
  10.  
    ...
  11.  
    }
  12.  
    ...
  13.  
    for (const auto &frame : buf) {
  14.  
    uint8_t len = frame.len;
  15.  
    uint32_t uid = frame.id;
  16.  
    const uint8_t *data = frame.data;
  17.  
    // 調用 ContiRadarMessageManager 的 Parse 方法解析雷達數據
  18.  
    pt_manager_->Parse(uid, data, len);
  19.  
    ...
  20.  
    }
  21.  
    }
  22.  
    }

這個方法會在 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

  1.  
    void Parse(const uint32_t message_id, const uint8_t *data, int32_t length)
  2.  
     

Parse 方法接收 3 個參數,消息ID,數據,數據長度。
拿到數據后,Parse 方法先通過消息 ID 從 protocol_data_map_ 中獲取了對應的數據處理對象。該方法並不會拿到數據后直接開始解析,而是需要判斷雷達的狀態信息,如果與用戶匹配才開始解析並發送消息。

  1.  
    // 如果雷達未配置,且消息 ID 不是 201 的話就結束 Parse 方法
  2.  
    if (!is_configured_ && message_id != RadarState201::ID) {
  3.  
    // read radar state message first
  4.  
    return;
  5.  
    }
  6.  
    // 如果消息 ID 是 600 或 60A 的時候則觸發發送
  7.  
    if (message_id == ClusterListStatus600::ID ||
  8.  
    message_id == ObjectListStatus60A::ID) {
  9.  
    ADEBUG << sensor_data_.ShortDebugString();
  10.  
     
  11.  
    if (sensor_data_.contiobs_size() <=
  12.  
    sensor_data_.object_list_status().nof_objects()) {
  13.  
    // maybe lost a object_list_status msg
  14.  
    AdapterManager::PublishContiRadar(sensor_data_);
  15.  
    }
  16.  
    sensor_data_.Clear();
  17.  
    // fill header when recieve the general info message
  18.  
    AdapterManager::FillContiRadarHeader(FLAGS_sensor_node_name, &sensor_data_);
  19.  
    }
  20.  
     
  21.  
    // 解析數據
  22.  
    sensor_protocol_data->Parse(data, length, &sensor_data_);
  23.  
     
  24.  
    // 否則消息是 201 ,則對比配置信息。
  25.  
    if (message_id == RadarState201::ID) {
  26.  
    ADEBUG << sensor_data_.ShortDebugString();
  27.  
    if (sensor_data_.radar_state().send_quality() ==
  28.  
    radar_config_.radar_conf().send_quality() &&
  29.  
    sensor_data_.radar_state().send_ext_info() ==
  30.  
    radar_config_.radar_conf().send_ext_info() &&
  31.  
    sensor_data_.radar_state().max_distance() ==
  32.  
    radar_config_.radar_conf().max_distance() &&
  33.  
    sensor_data_.radar_state().output_type() ==
  34.  
    radar_config_.radar_conf().output_type() &&
  35.  
    sensor_data_.radar_state().rcs_threshold() ==
  36.  
    radar_config_.radar_conf().rcs_threshold() &&
  37.  
    sensor_data_.radar_state().radar_power() ==
  38.  
    radar_config_.radar_conf().radar_power()) {
  39.  
    // 信息一致,雷達狀態已與用戶設置匹配
  40.  
    is_configured_ = true;
  41.  
    } else {
  42.  
    // 雷達狀態不匹配,再次配置雷達
  43.  
    AINFO << "configure radar again";
  44.  
    SenderMessage<ContiRadar> sender_message(RadarConfig200::ID,
  45.  
    &radar_config_);
  46.  
    sender_message.Update();
  47.  
    can_client_->SendSingleFrame({sender_message.CanFrame()});
  48.  
    }
  49.  
    }

我們發現有兩種特殊的消息 Id 需要處理。
一種是當消息 ID 為 201 的時候,201 是雷達的狀態信息,當拿到的是狀態信息的時候,需要判斷雷達狀態與用戶配置,如果不匹配則需要發送指令配置雷達。
第二種是消息 ID 為 600 或 60A,當消息 ID 為這兩種的時候,觸發消息的發布。


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM