在 多传感融合定位(五)—— autoware NDT单独编译与使用 中,将autoware定位建图有关的包拿出来单独编译,编译成功,接下来将进行包的测试,理清代码逻辑,最终在自己的车上进行实验,查看定位结果。
目录
1. 使用官方提供数据包进行建图与定位
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~的博客-CSDN博客_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格式
数据飞来飞去,很乱,以为是哪里出了问题,使用rviz可视化也一样。
下图是NDT_path: