我们已经了解了ROS地图的特征。现在我们假设已知在地图上某个位置有一台机器人，我们要将ROS地图转换为图片，同时机器人在地图上的位置传输至外部用户终端；使用户终端能够实时显示机器人位置。
我们先从用户终端（Windows小程序）的设计入手：
小程序使用Python结合pygame进行编程，在程序中要解决的三大问题：
1）机器人实际环境与像素位图的比例匹配
2）机器人相对于地图原点的配准
3）机器人的显示
1. 机器人在实际位置与位图位置的坐标变换和配准
首先，环境与位图的匹配公式相对比较简单：
def Env2Pix(x,y,ratio=0.05):
#环境到像素
return int(x/ratio),int(y/ratio)
def Pix2Env(x:int,y:int,ratio=0.05):
#像素到环境
return x * ratio, y * ratio
然后是机器人相对地图原点的配准，在这里，我们可以用一幅图片来表示：
现在我们已知在实际环境比例下地图图片的左下角位于坐标（a,b）处，而机器人位于坐标系(x,y)处。那么我们首先要做的就是将机器人在实际环境下应当处于地图的哪个位置
计算出来，我们在机器人相对地图位置不变的基础上，将坐标系原点移动到左下角（右图），那么现在，机器人的坐标位置变为了：（x-a,x-b）。
所以，我们在实际环境比例下，将机器人相对于地图原点位置变为相对于地图左下角位置的程序，可以简单记为：
def Coord2PicLeftDown(cornerCord_x,cornerCord_y,target_X,target_Y):
return target_X-cornerCord_x,target_Y-cornerCord_y
那么，将机器人实际坐标转化为机器人在图片上的像素位置的代码为：
def GetRobotPixel(real_x,real_y,p_X,p_Y,m_resolution=0.05,):
#首先，将所有环境坐标转换为图片像素坐标
pix_x,pix_y=Env2Pix(real_x,real_y,ratio=m_resolution)
origin_X,origin_Y=Env2Pix(p_X,p_Y,ratio=m_resolution)
#然后，获得机器人相对图片（0，0）点的坐标（注意，这是相对于图片右上角的）
return Coord2PicLeftDown(origin_X,origin_Y,pix_x,pix_y)
然后就可以将图片和机器人的位置显示在APP上了。
2. 图片点击位置与机器人实际位置的变换
我们先来看一下客户端与机器人端之间的坐标变换情况：
很多时候，显示在客户端上的图片是可以拖动的，那么以显示界面左上角为原点（浅蓝色框），我们可以点击一个位置，记为（x,y），那么，这个数据在以机器人为图片右下
角为原点的坐标系上，它的坐标应该为：（x-w,H+h-y)，具体各个变量参考图片。
那么我们就可以使用以下简单的代码还原机器人相对于ROS地图左下角点的坐标：
def AppPixPos2ROSPix_Pos(picCorner_x,picCorner_y,target_X,target_Y,picWidth,picHeight):
return target_X-picCorner_x,picHeight+picCorner_y-target_Y
转换完成后，就可以构建一个完整的鼠标/触摸点击—>机器人环境位置的链接：
def AppPixPos2Env(picCorner_x,picCorner_y,target_x,target_y,picWidth,picHeight,origin_Pix_x,origin_Pix_y,zoom_ratio=1,m_resolution=0.05):
#首先，根据点击位置和图片位置，将点击坐标转换为ROS形式的图片坐标
ROS_Bitmap_x,ROS_Bitmap_y=AppPixPos2ROSPix_Pos(picCorner_x,picCorner_y,target_x,target_y,picWidth,picHeight)
ROS_Bitmap_x/=zoom_ratio;ROS_Bitmap_y/=zoom_ratio
#然后，ROS相对图片坐标转换为相对ROS原点坐标
picCoord_x,picCoord_y= PicLeftDown2Coord(origin_Pix_x,origin_Pix_y,ROS_Bitmap_x,ROS_Bitmap_y)
# 最后，将坐标转换为实际环境坐标
return Pix2Env(picCoord_x,picCoord_y,m_resolution)
然后通过合理的显示以及http传输协议，就可以实现客户端显示与ROS之间的同步了。
欢迎大家关注，你的关注就是雨哥更新的动力~！
另外，本博客是雨哥在做项目过程中总结提炼的非涉密技术类文档，欢迎大家转发，转载请注明出处，同时不得用于商业用途！
