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

本文

主要介绍惯导的实车调试

调试流程

主要围绕能不能启动，能不能读到数据，数据对不对这三点展开。

启动gnss

首先在编译完gnss组件后，可以通过apollo自带的脚本启动它(在Apollo工作路径下)

bash scripts/gps.sh start_fe

通过后面加start_fe选项可以打印输出信息到终端。

如果串口打开失败可以用chmod为串口赋权限。

sudo chmod 777 /dev/ttyUSB0

调试数据读取

然后可以通过命令行工具cyber_monitor来查看gnss的消息是否解析到了

cyber_monitor

频率飞了，看来程序改得还是有问题。

分析下感觉像是线程的运行频率，怀疑是raw_stream的DataSpin()线程一直在疯狂读取串口。但是读取串口后加延时，效果适得其反。

改了半天，突然想到会不会是一直在解析的循环里没出来，果然在data_parser.cc文件里，看下ParseRawData()这个函数的解析逻辑。

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::ParseRawData(const std::string &msg) {
  if (!init_flag_) {
    AERROR << "Data parser not init.";
    return;
  }

  data_parser_->Update(msg);
  data_parser_->GetMessage();
  DispatchMessage();
}

然后编译完重新启动，看下cyber_monitor的内容。

Nice，问题解决！

调试数据解析

然后看下消息内容，timestamp_sec不对。

在data_parser.cc文件里，把PublishOdometry()这个函数的解析做如下修改。

void DataParser::PublishOdometry(const MessagePtr message) {
  Ins *ins = As<Ins>(message);
  auto gps = std::make_shared<Gps>();

  // double unix_sec = apollo::drivers::util::gps2unix(ins->measurement_time());
  gps->mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());
  auto *gps_msg = gps->mutable_localization();

  ...

还有PublishCorrimu()函数也做如下修改。

void DataParser::PublishCorrimu(const MessagePtr message) {
  Ins *ins = As<Ins>(message);
  auto imu = std::make_shared<CorrectedImu>();
  double unix_sec = apollo::drivers::util::gps2unix(ins->measurement_time());
  imu->mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());

现在消息数据应该正常了，然后有网的话可以通过Apollo的Dreamview在卫星地图上看下定位数据对不对。

首先在docker环境中通过如下命令启动它。

bash scripts/bootstrap.sh

然后在浏览器打开localhost:8888这个地址应该就能看到Dreamview的界面。

选择Navigation mode，就能在左上角看到百度地图提供的定位信息，然后也能看卫星地图。

Done！