在 多传感融合定位（五）—— autoware NDT单独编译与使用 中，将autoware定位建图有关的包拿出来单独编译，编译成功，接下来将进行包的测试，理清代码逻辑，最终在自己的车上进行实验，查看定位结果。

目录

1. 使用官方提供数据包进行建图与定位

1.1 建图测试

1.2 定位测试 

2.  使用自己的传感器进行定位与建图

2.1 数据采集

2.2 离线建图

2.3 实时定位

3.  定位结果分析

---

1. 使用官方提供数据包进行建图与定位

ROSBAG Demo · Wiki · Autoware Foundation / MovedToGitHub / autoware · GitLabProject has been moved to Github: https://github.com/Autoware-AI/autoware.aihttps://gitlab.com/autowarefoundation/autoware.ai/autoware/-/wikis/ROSBAG-Demo

1.1 建图测试

        先按照上面的demo下载解压对应的数据，接下来播放数据包，查看对应的数据包信息。

 

         可见，包内只提供了两种topic，GPS、点云；先来看下GPS信息：

sentence: "QQ02C,INSATT,V,004112.00,8.194,12.416,151.186,@65"
---
header: 
  seq: 156045
  stamp: 
    secs: 1427157687
    nsecs: 185960054
  frame_id: "/gps"
sentence: "WW03E,INSDAT,004112.00,1.623,-2.647,-10.231,-2.953,-2.487,1.610,@95"
---
header: 
  seq: 156046
  stamp: 
    secs: 1427157687
    nsecs: 334904909
  frame_id: "/gps"
sentence: "$GNRMC,004112.20,A,3514.0978980,N,13700.2997565,E,9.4358,147.001,240315,7.320,E,D*1F"
---
header: 
  seq: 156047
  stamp: 
    secs: 1427157687
    nsecs: 352860927
  frame_id: "/gps"
sentence: "$GPGGA,004112.20,3514.0978980,N,13700.2997565,E,4,12,0.82,47.7540,M,38.4589,M,1.2,0556*43"
---
header: 
  seq: 156048
  stamp: 
    secs: 1427157687
    nsecs: 362860918
  frame_id: "/gps"
sentence: "$GNVTG,147.001,T,154.321,M,9.4358,N,17.4751,K,D*09"
---
header: 
  seq: 156049
  stamp: 
    secs: 1427157687
    nsecs: 371870994
  frame_id: "/gps"

        里面有很多种消息类型，接下来运行建图节点：

roslaunch lidar_localizer ndt_mapping.launch 

        启动nmea转换节点：

roslaunch gnss_localizer nmea2tfpose.launch 

        播放数据：

rosbag play sample_moriyama_150324.bag

        rviz可视化建图过程：

        查看节点图：

 

         地图保存，在程序运行前，先运行下面的程序：

rosrun pcl_ros pointcloud_to_pcd input:=/ndt_map prefix:=map

        地图被不停保存到pcd文件中，该文件位于运行指令的目录下： 

         加载查看地图：

 

roslaunch map_file points_map_loader.launch path_pcd:="/home/sml/0.pcd"

         加载地图很慢，考虑降采样来减小地图，还有通过分割，将大地图分割成小地图。

 leaf size: 0.3 地图大小：19.7M, 效果：

 leaf size: 0.15 地图大小：499.8M, 效果：(加载还是快的，但是，rviz操作很卡)

 最终测试结果，leaf_size在0.255左右比较合适。

 

rosrun map_tools pcd_filter "PointXYZI" "0.255" "/home/sml/0.pcd" 

1.2 定位测试 

        autoware定位测试：

（1）播放数据

         （2）设置TF：主要设置雷达到base_link的静态坐标变换

（3）加载地图及TF

roslaunch map_file points_map_loader.launch path_pcd:="/home/sml/0.26_0.pcd" 

roslaunch lidar_localizer setup_tf.launch 

        TF文件设置了两个静态坐标变换：

<!---->
<launch>
<!-- worldからmapへのtf -->
<node pkg="tf"  type="static_transform_publisher" name="world_to_map" args="14771 84757 -39 0 0 0 /world /map 10" />

<!-- mapからmobilityへのtf -->
<node pkg="tf"  type="static_transform_publisher" name="map_to_mobility" args="0 0 0 0 0 0 /map /mobility 10" />

</launch>

（4）设置点云过滤：用于实时输入点云的过滤 

roslaunch points_downsampler points_downsample.launch

 

（5）设置NDT matching（个人GPU好像还是有问题，只能CPU运行）

(6) 播放数据，可视化定位

 （7）节点分析

（8）将gnss用于初定位

roslaunch gnss_localizer nmea2tfpose.launch 

roslaunch runtime_manager runtime_manager.launch

roslaunch lidar_localizer ndt_matching.launch

 ndt_stat信息：

 

        将上面的工作整理了一下，写了下面的launch文件，可以一次启动多个launch:

<launch>
<!-- 启动runtime_manager roslaunch runtime_manager runtime_manager.launch"-->
<!-- <include file="$(find runtime_manager)/launch/runtime_manager.launch">
</include> -->

<!-- 加载静态坐标变换：包括base_link<->velodyne,world<->map  -->
<include file="$(find lidar_localizer)/launch/setup_tf.launch">
</include>

<!-- 加载地图  roslaunch map_file points_map_loader.launch path_pcd:="/home/sml/0.pcd"-->
<include file="$(find map_file)/launch/points_map_loader.launch">
    <arg name="scene_num" default="noupdate" />
    <arg name="path_area_list" default='""' />
    <arg name="path_pcd" default='"/home/sml/0.26_0.pcd"' />
</include>

<!-- 启动滤波器  roslaunch points_downsampler points_downsample.launch"-->
<include file="$(find points_downsampler)/launch/points_downsample.launch">
    <arg name="sync" default="false" />
    <arg name="node_name" default="voxel_grid_filter" />
    <arg name="points_topic" default="points_raw" />
    <arg name="output_log" default="false" />
    <arg name="measurement_range" default="200" />
</include>

<!-- 启动nmea  roslaunch gnss_localizer nmea2tfpose.launch"-->
<include file="$(find gnss_localizer)/launch/nmea2tfpose.launch">
    <arg name="plane" default="7"/>
</include>

<!-- 启动NDT matching  roslaunch lidar_localizer ndt_matching.launch"-->
<include file="$(find lidar_localizer)/launch/ndt_matching.launch">
    <arg name="method_type" default="0" /> <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
    <arg name="use_gnss" default="1" />
    <arg name="use_odom" default="false" />
    <arg name="use_imu" default="false" />
    <arg name="imu_upside_down" default="false" />
    <arg name="imu_topic" default="/imu_raw" />
    <arg name="queue_size" default="1" />
    <arg name="offset" default="linear" />
    <arg name="get_height" default="false" />
    <arg name="use_local_transform" default="false" />
    <arg name="sync" default="false" />
    <arg name="output_log_data" default="false" />
    <arg name="gnss_reinit_fitness" default="500.0" />
</include>
</launch>

         上面的工作，运行只需要两个窗口：

roslaunch runtime_manager runtime_manager.launch

roslaunch lidar_localizer localization.launch

         然后播放数据，就能得到定位结果。

2.  使用自己的传感器进行定位与建图

无人驾驶Autoware代码中GNSS和激光雷达定位ndt_matching - 简书https://www.jianshu.com/p/be87ec155e15

2.1 数据采集

（1）启动驱动

roslaunch driver_launch driver.launch

（2）使用autoware来录制数据包（也可以使用下面的指令录制数据包）

# rosbag record /topic1 /topic12 -o out.bag
# rosbag record -a out.bag

（3）驱动分析

         测试驱动与定位程序一起运行：

         有个问题，nmea2tfpose包还是不输出pose信息，那就换一个（fix2tfpose）：

2.2 离线建图

(1) jetson 下构建的地图

（2）notebook下构建的地图

 

2.3 实时定位

(1)  使用/fix

---

2.4   关于NDT_matching参数说明

        NDT_matching参数在autoware中是通过runtime manager以话题的方式发布的，话题通过界面触发，每切换界面按钮、修改参数并确定就触发一次话题发布。

        参数说明：

• init_pos_gnss：1通过GNSS来确定初值，0通过初始位姿确定初值；
• use_predict_pose：1使用预测位姿，0不使用预测位姿；
• error_threshold: （1.0）该参数不知道干嘛的，好像没用；
• resolution: （0.1000000014）网格大小设置；
• step_size: (0.1000000014) 迭代步长；
• trans_epsilon: （0.00999999977648）算法收敛条件；
• max_iterations: （30）最大迭代次数；

        这些参数通过话题方式发布，来实时改变程序中的NDT配置参数，类似于动态参数配置，至于为什么不用ros中的动态参数配置，不知道。

        接下来的工作，可以将这部分参数写成一般参数，在程序开始时加载。

3.  定位结果分析

evo测评TUM数据集_dididada~的博客-程序员宅基地_evo tumhttps://blog.csdn.net/qq_43265072/article/details/104715515

使用evo评测VIO算法 - 简书https://www.jianshu.com/p/a7a3f93bfbba（1）安装evo

pip3 install evo --upgrade

（2）保存轨迹为TUM格式

ROS轨迹保存为tum格式,并用evo轨迹绘制_just_do_it567的博客-程序员宅基地https://blog.csdn.net/just_do_it567/article/details/114672305(3）绘制数据图

        数据飞来飞去，很乱，以为是哪里出了问题，使用rviz可视化也一样。

下图是NDT_path: