Apollo详解之定位模块——NDT定位

前言

代码分析

NDTLocalizationComponent

NDTLocalizationComponent::Init

该函数完成了NDT组件的初始化工作。该组件通过命令行参数gflag对相关参数进行初始化，各命令行参数的默认值可参考modules/common/adapters/adapter_gflags.cc和modules/localization/common/localization_gflags.cc，该组件读取命令行参数配置文件modules/localization/conf/localization.conf，因此可在该文件中对各命令行参数进行定义。

命令行参数

话题相关

• FLAGS_localization_topic 融合定位发布话题
• FLAGS_lidar_topic 激光雷达点云订阅话题
• FLAGS_localization_ndt_topic NDT定位发布话题
• FLAGS_ins_stat_topic 组合导航定位状态订阅话题
• FLAGS_localization_msf_status 融合定位状态发布话题
  变换相关
• FLAGS_broadcast_tf_frame_id 固定世界坐标系
• FLAGS_broadcast_tf_child_frame_id 子坐标系
  融合相关
• FLAGS_local_utm_zone_id 定位地图UTM坐标系ID
• FLAGS_online_resolution
• FLAGS_ndt_debug_log_flag 是否记录日志
• FLAGS_lidar_height_file 激光雷达高度YAML配置文件
• FLAGS_lidar_extrinsics_file 激光雷达外参YAML配置文件
• FLAGS_ndt_bad_score_count_threshold
• FLAGS_ndt_warnning_ndt_score
• FLAGS_ndt_error_ndt_score
  定位地图相关
• FLAGS_map_dir 定位地图文件夹路径
• FLAGS_ndt_map_dir ndt地图相对路径
• FLAGS_local_map_name 定位地图相对路径

NDTLocalization::Init

该函数会被调用，读取激光雷达外参和激光雷达高度参数以及定位地图

配置参数文件

velodyne128_novatel_extrinsics.yaml

header:
  stamp:
    secs: 1542235619
    nsecs: 0
  frame_id: novatel
  seq: 0
child_frame_id: velodyne128
transform:
  rotation:
    x: 0.007209096210038245
    y: 0.01691200555753232
    z: 0.7076293956185187
    w: 0.7063445699206772
  translation:
    x: 0.03614259942391
    y: 1.033871037991605
    z: 1.21285

velodyne128_height.yaml

vehicle:
  parameters:
    height: 1.89
    height_var: 0.0045

回调函数

LidarCallback

点云消息的回调函数，获得点云消息，进行计算后得到根据NDT的定位消息，并进行发布。

OdometryStatusCallback

组合导航状态回调函数，获得导航状态，存储在相关容器中

NDTLocalizationComponent::Proc

Gps定位消息的回调函数，根据Gps定位的时间戳，从容器中获得相应的导航状态，根据生成的NDT定位消息获得定位参数，再结合Gps的速度加速度信息，得到融合定位消息和相应的状态，进行发布，并根据定位信息发布世界坐标系和定位坐标系的动态坐标变换