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 的监听。

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 
void CanReceiver::RecvThreadFunc() {
  ...
  while (IsRunning()) {
    std::vector 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 sender_message(RadarConfig200::ID,
                                               &radar_config_);
      sender_message.Update();
      can_client_->SendSingleFrame({sender_message.CanFrame()});
    }
}

我们发现有两种特殊的消息 Id 需要处理。
一种是当消息 ID 为 201 的时候，201 是雷达的状态信息，当拿到的是状态信息的时候，需要判断雷达状态与用户配置，如果不匹配则需要发送指令配置雷达。
第二种是消息 ID 为 600 或 60A，当消息 ID 为这两种的时候，触发消息的发布。