Apollo详解之驱动模块———组合导航驱动

前言

组合导航驱动的作用是读取组合导航发送的数据，进行解析后得到相应的消息类型，并将其发布至相关话题。
目前Apollo只支持星网宇达Newton-M2驱动。

使用方法

根据实际修改配置文件modules/drivers/gnss/conf/gnss_conf.pb.txt
启动驱动

cyber_launch start modules/drivers/gnss/launch/gnss.launch

代码简要分析

组合导航也是通过Cyber_RT框架的Component调用的，其包含一个成员RawStream成员。首先在GnssDriverComponent::Init函数中根据配置参数创建一个RawStream对象。

RawStream

该类是的功能是驱动组合导航，从设备接口读取原始数据，然后将其解析后发布至相应的话题。

主要的数据成员：

• 包含多个Stream类对象以及相应的Status类对象，负责读取原始数据和记录其状态；Stream类对象有：
  ·data，负责读取导航数据
  ·command，负责向组合导航设备发送数据，默认与data使用相同的端口
  ·rtk_from，负责读取RTK矫正数据
  ·rtk_to，负责向组合导航设备发送RTK矫正数据，默认与rtk_from使用相同的端口
• 包含一个DataParser和一个RtcmParser，分别负责将原始数据解析为相应的消息类型并发布至相关话题。

构造函数

分别创建DataParser和RtcmParser

RawStream::Init

初始化DataParser和RtcmParser，并创建配置参数中各个Stream对象及相应的状态，连接所有Stream对象并登录，然后订阅底盘话题，订阅原始数据话题保存至文件中

RawStream::Start

开启两个线程，根据参数开启定时调用函数。
从导航设备流中读取数据，解析为对应的消息，发布至相应的话题。从RTK设备流中读取数据，解析为对应的消息，发布至相应的话题。定时从底盘接收车速信息，将其发布至控制设备流。

Stream

流类对象的基类，其派生类有TcpStream、UdpStream、SerialStream、NtripStream。分别负责根据不同的协议读取二进制字节数据。

创建方法

工厂方法create_stream，根据配置参数中Stream.type创建对应的流类对象Stream。

主要方法

Connect

打开指定的设备，并对设备相应的设置

read

从指定设备读取指定字节数的数据

write

向指定设备发送指定字节数的数据

Parser

解析器的基类，其派生类有NovatelParser、Rtcm3Parser。分别负责解析原始数据，得到不同种类的消息。

创建方法

工厂方法CreateParser，根据配置参数中Stream.Format创建对应的解析器对象。

主要数据成员

data_和data_end_两个指针，这两个指针中间的内存即代表要解析的原始数据。

主要方法

Update

更新解析器的原始数据

GetMessage

解析原始数据，将其解析为对应的消息类型

DataParser

导航数据解析类。负责解析原始二进制数据，得到不同种类的导航消息，然后将其发布至相关话题。

发布的话题

• /apollo/sensor/gnss/raw_data 导航原始数据
• /apollo/sensor/gnss/gnss_status 卫星导航状态
• /apollo/sensor/gnss/best_pose 大地坐标系下的GNSS定位信息
• /apollo/sensor/gnss/imu ENU坐标系下的IMU消息
• /apollo/sensor/gnss/ins_status 惯性导航状态
• /apollo/sensor/gnss/corrected_imu ENU坐标系下的IMU消息
• /apollo/sensor/gnss/odometry ENU坐标系下定位消息
• /apollo/sensor/gnss/ins_stat InsStat
• /apollo/sensor/gnss/rtk_eph GNSS卫星位置表信息
• /apollo/sensor/gnss/rtk_obs 一轮的观测结果
• /apollo/sensor/gnss/heading Heading
• /apollo/sensor/gnss/stream_status 各个设备流状态

原始数据格式

novatel::SYNC_0 novatel::SYNC_1 SYNC_2_LONG_HEADER或SYNC_2_SHORT_HEADER message

注意其中的空格只是方便阅读，实际并不包含空格。
每个元素数据都包含一个首部，包含元素数据的长度，数据类型、gps_week、gps_millisecs。
message代表导航数据，存在不同的类型，如GPS_Ephemeris、BestPos等等，具体定义可参考modules/drivers/gnss/parser/novatel_messages.h

主要数据成员

• 各个话题的发布者
• 一个解析器接口Parser

主要方法

ParseRawData

不断解析组合导航数据,并发布至相应的话题

RtcmParser

RTK数据解析类。负责解析不同的RTK消息，然后将其发布至相关话题。

发布的话题

• /apollo/sensor/gnss/rtk_eph 发布GNSS卫星位置表信息
• /apollo/sensor/gnss/rtk_obs 发布一轮观测信息

主要数据成员

• 各个话题的发布者
• 一个解析器接口Parser

主要方法

ParseRtcmData

不断解析组合导航数据,并发布至相应的话题

坐标系转换

Apollo组合导航中涉及多个坐标系如WGS-84地心坐标系、基于UTM下的ENU本地坐标系、车辆的FLU坐标系等，因此位置、线速度、线加速度、角速度、欧拉角等物理量需要进行坐标系之间的转换。

WGS-84地心坐标系转换为基于UTM下的ENU本地坐标系

在DataParser::PublishOdometry中

  double x = ins->position().lon();  // WGS-84地心坐标系转
  double y = ins->position().lat();  // WGS-84地心坐标系转
  x *= DEG_TO_RAD_LOCAL;
  y *= DEG_TO_RAD_LOCAL;

  pj_transform(wgs84pj_source_, utm_target_, 1, 1, &x, &y, NULL);

  gps_msg->mutable_position()->set_x(x);  // 基于UTM下的ENU本地坐标系
  gps_msg->mutable_position()->set_y(y);  // 基于UTM下的ENU本地坐标系
  gps_msg->mutable_position()->set_z(ins->position().height());

车辆的FLU坐标系转换为ENU本地坐标系

FLU坐标系中，xyz轴分别代表车辆前方，车辆左方，车辆上方。
ENU坐标系中，xyz轴分别代表东方，北方，上方。

线速度、线加速度

由于ENU坐标系的x轴代表东方，因此等于FLU坐标系y轴（车辆左方）取反后的结果。
ENU坐标系y轴代表北方，因此等于FLU坐标系x轴（车辆前方）的结果。
ENU坐标系z轴代表上方，等于FLU坐标系上方。
因此在DataParser::PublishImu中

  raw_imu->mutable_angular_velocity()->set_x(-imu->angular_velocity().y());
  raw_imu->mutable_angular_velocity()->set_y(imu->angular_velocity().x());
  raw_imu->mutable_angular_velocity()->set_z(imu->angular_velocity().z());

角速度

同上

欧拉角

由于FLU坐标系转换为ENU坐标系的过程中，x-y平面顺时针的旋转了90°，因此绕z轴的旋转yaw的0°方向相应顺时针旋转了90°，所以需要减90°；而x-z和y-z方向没有发生旋转，因此不需要发生变化。