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 为这两种的时候，触发消息的发布。