ros中如何根据map.yaml和tf数据确定地图中机器人的位置

地图的yaml格式中有

其中origin是建图时机器人的初始位置，单位是米m

问题1：如何将orgin转换成实际地图中的位置？

1.确定地图的坐标系，为最右上角的像素为坐标（0,0）

整副地图都处于坐标系的第三像限

2.解析origin x=-2.5m y=-1.6m,将x，y的值除以分辨率resolution（米/像素）

得出x=-50个像素，y为-32个像素

从地图的最右上角像素数，往左数第50个像素，再往下数第32个像素，就是orgin所代表的机器人初始位置。

问题2：如何在导航中实时显示机器人的位置

1.确定机器人本身的坐标系，一般是base_footprint或者base_link，在2d的地图上这两者基本没有区别，可选取其中一个

2.获取从map到base_footprint的tf变换，代表机器人以orgin位置为原点的坐标系上位置。

3.orgin中的x,y（减去/加上）tf变换中的x，y，就是当前机器人的当前位置，再除以resolution，就是地图中机器人位置所在的像素。