机器人区域映射Python

最近我一直在研究Java中的机器人项目.机器人被指示在房间周围驾驶记录有关其周围区域的数据.机器人在机器人周围有16个声纳传感器.我编写的Java代码与我的问题无关,但它所做的只是驱动机器人并将数据写出CSV文件.

传感器定位如下:

该文件如下所示: 此处输出文件

CSV文件中的每一行都包含以下信息:xpos,ypos,yaw(弧度),16个声纳读数,时间戳

机器人启动时,它位于右上角.机器人将世界视为20x20网格(-10到10).

我正在使用python来处理这些信息,我正在尝试映射机器人周围的环境.当CSV文件告诉我世界上的机器人x,y位置和机器人偏航时,我想使用python图像库绘制图像上的机器人位置,然后在距指定距离的机器人周围添加一个红点.声纳传感器.

目前我有这个代码来生成机器人路径:

with open('data.csv') as fp:
    for line in fp:
        tempLine = line.split(',')
        x = (float(tempLine[0])+10.0)*100
        y = (float(tempLine[1])+10.0)*100

        idraw.rectangle([(x,2000-y), (x+10,2000-(y+10))], fill=(0,0,0)) 

给出以下输出:

我正试图让声纳点映射失败.我正在研究传感器距离的位置,然后旋转机器人偏航周围的坐标.

这根本没用,给了我一些相当奇怪的结果:

任何帮助将不胜感激!谢谢!