Apollo惯导驱动适配Step By Step(上)

2023-12-01 # 实用教程 #Autonomous Driving #Apollo #Localization #C/C++

本文

主要介绍如何为Apollo快速适配一个新的惯导，我这里用的是导远电子的INS570D组合惯导。驱动可以在我的github仓库里找到，Ins570d repo。

关于INS570D

我们主要关注其物理接口与输出数据格式定义，一个关系到惯导驱动里的stream节点，另一个则是parser节点。下图是这款惯导的接口图

可以看出我们需要从422串口读取惯导原始数据，跟图上稍有不同的是，我这边用422转USB的线接到了测试电脑上。

然后是发过来的数据格式定义，一般都是NMEA格式，但是导远发的是解算好的十六进制数据，所以后续解析我们需要根据下图这个手册来。

方案选择

由于Apollo本身自带有Novatel的惯导驱动，所以适配新惯导就有了两种方案

把新惯导解析出来的数据对接到Apollo自带的驱动上
重头写一个惯导解析驱动

从可行性角度分析，两者都OK，方案二会好点。但显然本文选择了方案一，仅仅出于工作量的考虑。

适配流程

接下来就跟我一步步适配吧，大致分为认识代码结构、修改对应接口、实际测试三部分。

惯导目录

首先，我们一起来看下惯导驱动所在的工程目录（/module/drivers/gnss）,这里是apollo的工作根目录。

DIR/FILE	Description
conf(*)	存放惯导解析驱动的配置文件
dag/launch	启动惯导解析程序的文件
parser(*)	解析惯导数据的源码文件
proto(*)	存放消息定义的文件
stream(*)	读取串口原始惯导数据的源码文件
test/test_data	单元测试文件
util	存放一些公共头文件
BUILD(*)	相当于CmakeList文件，用于编译gnss驱动
gnss_component.cc/h	惯导驱动的程序入口

注：文件目录末尾带(*)的，是我们适配新的惯导需要着重关注进行修改的文件。

输出通道

以下通道后带(*)的数据是必须要输出的，因为localization模块需要用到这些消息，其他下表列出来的通道也尽可能输出出来，方便调试。

Channels	Description
/apollo/sensor/gnss/raw_data	组合惯导原始数据
/apollo/sensor/gnss/imu	imu原始数据
/apollo/sensor/gnss/best_pose	组合惯导最佳定位数据
/apollo/sensor/gnss/odometry(*)	组合惯导的位姿和线速度
/apollo/sensor/gnss/corrected_imu(*)	校正IMU,即原始IMU数据去除了重力和bias
/apollo/sensor/gnss/ins_stat(*)	组合惯导的定位状态,决定最终定位的状态

然后我们只要着重把惯导解析出来的数据尽量接到这些通道上，这里用‘尽量’，是因为有些消息我也没法对上，但是 it work 了，所以尽量就行。

另外由于导远本身发过来的数据就是矫正后的定位信息，所以rtk_xx这些通道的数据就不用管了，相应的parser目录里的那些带有rtk的文件也不用去动它了。

Stream

我们需要从串口中读数据，首先就得创建这个stream，我们需要关注的代码在raw_stream.cc的RawStream::Init()这个成员函数里

// Creates streams.
Stream *s = nullptr;
if (!config_.has_data()) {
  AINFO << "Error: Config file must provide the data stream.";
  return false;
}
s = create_stream(config_.data());

create_stream就是创建stream的函数，他的原型定义在Stream *create_stream(const config::Stream &sd) 下，里面也包含了RTK数据流的创建，我们主要关注如下这部分代码。

case config::Stream::kSerial:
      if (!sd.serial().has_device()) {
        AERROR << "Serial def has no device field.";
        return nullptr;
      }
      if (!sd.serial().has_baud_rate()) {
        AERROR << "Serial def has no baud_rate field. Use default baud rate "
               << sd.serial().baud_rate();
        return nullptr;
      }
      return Stream::create_serial(sd.serial().device().c_str(),
                                   sd.serial().baud_rate());

上面这段代码主要读取gnss_conf_pb.txt中data字段中的serial配置，要适配新的惯导就需要将下面这个配置文件内容与设备对应上。

data {
    format: INS570D_BINARY
    serial {
        device: "/dev/ttyUSB0"
        baud_rate: 115200
    }
}

如我这里用的usb串口在linux下挂载在/dev/ttyUSB0路径上，惯导波特率设置的是115200。另外format这个配置其实改不改都不影响，但是为了方便跟源惯导区分，我就改成INS570D_BINARY了。主要串口路径与波特率设置好就行。

然后就到RawStream::DataSpin()这个函数下了，它只是个负责读取串口数据的线程，所以我们主要关注这个while()循环里的内容。

// Origin Code
while (cyber::OK()) {
    size_t length = data_stream_->read(buffer_, BUFFER_SIZE);
    if (length > 0) {
      std::shared_ptr<RawData> msg_pub = std::make_shared<RawData>();
      if (!msg_pub) {
        AERROR << "New data sting msg failed.";
        continue;
      }
      msg_pub->set_data(reinterpret_cast<const char *>(buffer_), length);
      raw_writer_->Write(msg_pub);
      data_parser_ptr_->ParseRawData(msg_pub->data());
      if (push_location_) {
        PushGpgga(length);
      }
    }
    StreamStatusCheck();
}

这段代码的逻辑很清晰，就是通过read()方法从数据流中读取BUFFER_SIZE个数据到buffer_中，有数据就写到相应的通道里，然后就给到Parser节点做数据解析。那我们的话就只要做如下修改。

首先在raw_stream.h头文件中将static constexpr size_t BUFFER_SIZE = 2048;这里的2048改成63。因为我用的惯导每次发送(56+7)Byte的数据，其中56Byte的数据位，7Byte的校验位;

其次可以将PushGpgga()那段逻辑删了，因为导远INS570D设备完全不会发送这数据，所以最终修改如下。

//  Modified
while (cyber::OK()) {
  size_t length = data_stream_->read(buffer_, BUFFER_SIZE);
  if (length == BUFFER_SIZE) {
    std::shared_ptr<RawData> msg_pub = std::make_shared<RawData>();
    if (!msg_pub) {
      AERROR << "New data sting msg failed.";
      continue;
    }
    msg_pub->set_data(reinterpret_cast<const char *>(buffer_), length);
    raw_writer_->Write(msg_pub);
    data_parser_ptr_->ParseRawData(msg_pub->data());
  }
  StreamStatusCheck();
}

Parser

接下来就到正式的Parser解析部分了。
首先来看data_parser.cc文件中的DataParser::DataParser()构造函数，其中wgs84到utm的投影设置是我们需要关注的内容。

std::string utm_target_param;

wgs84pj_source_ = pj_init_plus(WGS84_TEXT);
utm_target_ = pj_init_plus(config_.proj4_text().c_str());

根据上面这段代码，我们可以在gnss_conf_pb.txt文件中找到下面这行配置，这里+zone=10需要修改成我们使用惯导所在的区域。

1	proj4_text: "+proj=utm +zone=10 +ellps=WGS84 +towgs84=0,0,0,0,0,0,0 +units=m +no_defs"

然后继续往下，可以看到DataParser::Init()这个初始化函数里，有创建Parser的代码。

1	data_parser_.reset(CreateParser(config_, false));

这个CreateParser()的定义如下。

// Origin Code
Parser *CreateParser(config::Config config, bool is_base_station = false) {
  switch (config.data().format()) {
    case config::Stream::NOVATEL_BINARY:
      return Parser::CreateNovatel(config);

    default:
      return nullptr;
  }
}

很明显这需要跟我在配置文件里修改的参数对应上，所以修改如下。

// Modified
Parser *CreateParser(config::Config config, bool is_base_station = false) {
  switch (config.data().format()) {
    case config::Stream::INS570D_BINARY:
      return Parser::CreateIns570d(config);

    default:
      return nullptr;
  }
}

接着看CreateNovatel()这个函数，它在novatel_parser.cc文件里，定义如下。

// Origin Code
Parser* Parser::CreateNovatel(const config::Config& config) {
  return new NovatelParser(config);
}

到这里我才发现自己干了件挺awful的事，为了把NovatelParser换成Ins570dParser，我需要把novatel_parser.cc文件里所有的NovatelParser域改成Ins570dParser，不过也有必要，这样毕竟之后使用就不会有歧义了。

// Modified
Parser* Parser::CreateIns570d(const config::Config& config) {
  return new Ins570dParser(config);
}

那索性把novatel_XXX的.cc和.h文件名也改成了ins570d_XXX。
这样在ins570d_parser.cc文件里需要把头文件的路径也改下，还有ins570d_messages.h里的namespace需要把novatel改成Ins570d，还有parser.h里CreateNovatel()也需要改下。最后这些修改完之后，别忘了修改当前路径下的BUILD文件。
改完可以在Apollo的docker环境中，用下面这条命令编译下gnss驱动，如果报错按提示及时修改，因为只是改了名字不至于出什么大问题。

./apollo.sh build_opt drivers/gnss

然后重新回到data_parser.cc文件看到ParseRawData()这个函数，他主要读取一次串口数据就用GetMessage()和DispatchMessage()方法解析、发布消息。
这里有个问题在while()代码段下，它解析到一条有用数据就会return出来然后publish消息，这样程序完全是串行的。如果本身串口消息发送频率为50Hz的话，10种类型的消息每种就5Hz的频率了。这对定位模块来说肯定是不合理的。

data_parser_->Update(msg);
Parser::MessageType type;
MessagePtr msg_ptr;

while (cyber::OK()) {
  type = data_parser_->GetMessage(&msg_ptr);
  if (type == Parser::MessageType::NONE) {
    break;
  }
  DispatchMessage(type, msg_ptr);
}

不过不急一步步来，先看GetMessage()函数，他现在定义在ins570d_parser.cc文件下，主要是对原始数据Header稍微分析下，然后根据Header来读数据，读完给PrepareMessage()做数据解析。因为Novatel惯导会发两种不同长度的消息，但导远的数据长度是固定的，所以只要header对应上就行。

while (data_ < data_end_) {
  if (buffer_.empty()) {  // Looking for SYNC0
    if (*data_ == Ins570d::SYNC_0)
      buffer_.push_back(*data_);
    ++data_;
  } else if (buffer_.size() == 1) {  // Looking for SYNC1
    if (*data_ == Ins570d::SYNC_1)
      buffer_.push_back(*data_++);
    else
      buffer_.clear();
  } else if (buffer_.size() == 2) {  // Looking for SYNC2
    if (*data_ == Ins570d::SYNC_2)
      buffer_.push_back(*data_++);
    else 
      buffer_.clear();
  } else if (total_length_ >= 3) {
    if (buffer_.size() < total_length_) {  // Working on body.
      buffer_.push_back(*data_++);
      continue;
    }
    MessageType type = PrepareMessage(message_ptr);
    buffer_.clear();
    if (type != MessageType::NONE)
      return type;
  }
}

然后还需要对ins570d_parser.cc和ins570d_messages.h这两个文件做以下修改。

// ins570d_parser.cc的Ins570dParser类定义下
size_t total_length_ = 63;

// ins570d_messages.h下
enum SyncByte : uint8_t {   //跟手册定义的数据头格式对应
  SYNC_0 = 0xBD,
  SYNC_1 = 0xDB,
  SYNC_2 = 0x0B,
};

接下来看PrepareMessage()这个函数，这里就到正式的解析流程了。
首先有两个设计目标需要确认下

ins570d数据格式的声明与定义放哪
ins570d解析逻辑如何设计

关于第一点，因为novatel_messages.h（现在为ins570d_messages.h）已经定义了许多消息结构体，再把ins570d的数据格式声明在这里会很乱，所以我外面加了个INSD_Define.h文件放它的声明以及需要用到的一些变量定义都放这了，现在parser目录结构如下。

./parser/
├── BUILD
├── data_parser.cc
├── data_parser.h
├── INS570D_Define.h
├── ins570d_messages.h
├── ins570d_parser.cc
├── parser.h
├── rtcm3_parser.cc
├── rtcm3_parser.h
├── rtcm_decode.h
├── rtcm_parser.cc
└── rtcm_parser.h

但是ins570d解析数据的定义还是放ins570d_messages.h里了，然后在ins570d_messages.h里include新加的INSD_Define.h文件即可。

// under ins570d_messages.h
#include "INS570D_Define.h"

struct INS_STATUS {
  INS_STATUS() {}
  bool updated = false;
  uint8_t header_1 = 0;
  uint8_t header_2 = 0;
  uint8_t header_3 = 0;
  /*...*/

这里INS_STATUS结构体是根据惯导能解析出来哪些数据定义的，由于比较长就没展开放进上面代码块里了，不过完整代码可在我的github仓库里找到。

然后关于第二点，我是这么考虑的，INS570D每次发一次数据，buffer_就会包含我们所有想要的信息。那我们只要一次性把它从buffer_里全部解析出来即可。

这样当然不错，但是PrepareMessage()这个函数原本逻辑是解析到某一类型消息就调用HandleXXX()做对应的解析然后就return到DataParser::ParseRawData()里执行DispatchMessage()函数，他会根据PrepareMessage()返回的消息类型选择对应的PublishXXX()函数完成消息的发布。

就导远的惯导来说，程序这样设计是比较合理的，在PrepareMessage()函数里

1
2
3

HandleXXX();
...
HandleXXX();

然后在PrepareMessage()函数里

1
2
3

PublishXXX();
...
PublishXXX();

这就需要把原来解析好的数据定义给这两个函数共享，好在两者都include了parser.h头文件，那就对这个文件添加如下内容即可，注意变量定义需要放在apollo::drivers::gnssnamespace下。

#include "modules/drivers/gnss/proto/gnss.pb.h"
#include "modules/drivers/gnss/proto/gnss_best_pose.pb.h"
#include "modules/drivers/gnss/proto/gnss_raw_observation.pb.h"
#include "modules/drivers/gnss/proto/heading.pb.h"
#include "modules/drivers/gnss/proto/imu.pb.h"
#include "modules/drivers/gnss/proto/ins.pb.h"

namespace apollo {
namespace drivers {
namespace gnss {

Gnss gnss_;
GnssBestPose bestpos_;
Imu imu_;
Ins ins_;
InsStat ins_stat_;
GnssEphemeris gnss_ephemeris_;
EpochObservation gnss_observation_;
Heading heading_;

修改完我编译了下没过，由于把变量定义在全局域下跟test里的代码冲突了，索性把test都删了，编译通过。数据对象确定了，然后就是算法逻辑修改了。
先是到data_parser.cc文件下，对ParseRawData()跟DispatchMessage()函数做如下修改。

void DataParser::ParseRawData(const std::string &msg) {
  if (!init_flag_) {
    AERROR << "Data parser not init.";
    return;
  }

  data_parser_->Update(msg);
  while (cyber::OK()) {
    data_parser_->GetMessage();
    DispatchMessage();
  }
}

void DataParser::DispatchMessage() {
  MessagePtr message;
  message = &gnss_;
  CheckGnssStatus(As<::apollo::drivers::gnss::Gnss>(message));
  message = &bestpos_;
  PublishBestpos(message);
  message = &imu_;
  PublishImu(message);
  message = &ins_;
  CheckInsStatus(As<::apollo::drivers::gnss::Ins>(message));
  PublishCorrimu(message);
  PublishOdometry(message);
  message = &ins_stat_;
  PublishInsStat(message);
  message = &heading_;
  PublishHeading(message);
}

然后到ins570d_parser.cc文件下对PrepareMessage()函数做如下修改。

void Ins570dParser::PrepareMessage() {
  if (!check_sum()) {
    AERROR << "CRC check failed.";
    return ;
  }
  // split raw data
  dummy_array_.clear();
  std::vector<uint32_t> dummy_array(27);
  for (int i=0;i<27;i++)
    dummy_array[i]=getOneValue<uint32_t>(Ins570d::config_[i][0], Ins570d::config_[i][1]);
  dummy_array_ = dummy_array;
  
  // parse data
  Ins570d::INS_STATUS *tmp_=&(this->ins570d_data_);
  getRPY(*tmp_);
  getGYRO(*tmp_);
  getACC(*tmp_);
  getWGS84(*tmp_);
  getVEL(*tmp_);
  getALISTATUS(*tmp_);
  getLOOPDATA(*tmp_);
  ins570d_data_=*tmp_;
  // debug
  ins570d_data_.debugString();

  HandleGnssBestpos();
  HandleBestPos();
  HandleCorrImuData();
  HandleRawImu();
  HandleInsPvax();
  HandleHeading();
}

其中HandleXXX()函数上面部分是ins570d解析代码，HandleXXX()负责对接原有消息类型，为此我也对其做了重构，以HandleHeading()为例来看下。

bool Ins570dParser::HandleHeading() {
  // heading_.set_solution_status(Ins570d::SolutionStatus::SOL_COMPUTED);
  heading_.set_position_type(SolutionType::NARROW_INT);
  // heading_.set_baseline_length(heading->length);
  heading_.set_heading(ins570d_data_.yaw);
  heading_.set_pitch(ins570d_data_.pitch);
  // heading_.set_reserved(heading->reserved);
  /* ... */
  return true;
}

我取消了原有的传入参数，通过我解析出来的数据直接赋值，然后没有的消息类型直接注释掉不管即可。

至此

适配工作大致完成，内容有点多，所以剩下测试部分内容放到下篇文章讲了。

参考资料

apollo官方repo里提供的how_to_add_a_gps_receiver指南
apollo官方repo里提供的离线仿真demo，跑起来可以用Cyber_monitor对下gnss相关消息的数据格式

2023-12-01 13:51:59 # 实用教程 #Autonomous Driving #Apollo #Localization #C/C++