在AMCL/Gazebo中增加Ground Truth Pose作为验证

写在前面：本文为原创，如需转载请注明出处(https://www.jianshu.com/p/7858b30a50cd)。欢迎大家留言共同探讨，有误的地方也希望指出。另如果有好的SLAM、ROS等相关交流群也希望可以留言给我，在此先谢过了。
参考：
1: How To: Get ground truth pose data in ROS Gazebo
2: 代码
3: Rviz: fixed frame [world] does not exist

1 目标

在Gazebo中可以使用模拟的机器人和预先建立好的地图进行AMCL的定位算法模拟。但普通的AMCL是没有机器人的实际位置输出，仅有机器人的位姿输出，难以获得机器人在定位过程中的误差变化然后进行相应优化。因此需要增加一个topic来显示机器人在该地图上的真实位置，即Ground truth pose信息。

2 实现

根据1中的目标，经过一系列查找发现参考1中的文章提供了靠谱的解决方案，即通过在URDF文件中增加一个Plugin，发布新的地图topic /ground_truth/state。 具体的代码在参考2中，复制后添加在所用的URDF文件末尾即可。其会发布一个world frame和base_link的转换。
调整好后，gazebo与amcl，在rviz中出现如下故障：

rviz截图

发现并未有world frame。通过 参考3，想到解决方向：建立新的static_transform_publisher。根据现在情况，需要的是world->bas_link体系的坐标转换，因此根据实际情况添加world->map的坐标转换。

<node pkg="tf" type="static_transform_publisher" name="world_map" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 40" />

运行后，可以正确得到1中要求的目标。

带有ground truth的amcl定位

如图，蓝色为机器的Ground truth，红色箭头为机器人的amcl定位位姿结果。

ps. 因给出的默认Ground truth显示模式是用红色的里程计模型显示的，推荐改为其他颜色，与机器人的amcl_pose结果作为区分比对。