map与costmap都是以nav_msgs::OccupancyGrid类型发布其topic。其中整张地图的障碍物信息存放在data数据成员中,data是一个int8类型的vector向量,即一维数组。假设一张pgm的map地图,宽:width,高:height,单位为像素,分辨路为resolution,左下角像素点在世界坐标系下的位置为:(ox,oy),单位米,那么世界坐标系下一点(x,y)单位米,假设其在地图中,那么该点对应的data中的索引index为:
index = (x-ox)/resolution+(y-oy)/resolution*width
(注:index为int8类型,所以要将结果进行下类型转换),