ros map数据坐标变换

坐标系

坐标计算：

图片坐标系下的实际坐标（-10, -26）单位m(非ros坐标，如yaml中的origin)
宽：384px
长：704px
分辨率：0.05m/pxiel

图片坐标系转像素坐标系

Px = 10/0.05 = 200px;
Py = 26/0.05 = 520px;

像素其实是一个小方块，在ros map中，以方块右上角为点，如下图

故ros的像素坐标计算应为：

Px = x/resolution-1 = 199px;
Py = y/resolution-1= 519px;

在实际编程中，我们通常只能获得基于地图坐标系的坐标（全局坐标）,并不能简便的获得图片坐标。为了获得对应的像素坐标或是图片坐标，我们可以依靠Costmap_2d::Costmap2D中的worldToMap函数来计算获得：

地图坐标——>像素坐标

#include <costmap_2d/costmap_2d.h>
unsigned int mx;//像素坐标x
unsigned int my;//像素坐标y
costmap_2d::Costmap2D master_grid;
master_grid.worldToMap(mark_x_, mark_y_, mx, my);//mark_x_, mark_y_为地图坐标

像素坐标——>图片坐标

x = -(Px+1)*resolution
y = -(Py+1)*resolution

nav_msgs/OccupaidGrid计算

该msg中的data储存的ros像素坐标（199px,519px）对应的data[]数组序号计算
index = Py*width+Px
即，像素坐标(199px, 519px)对应的数组序号为
519 * 384+199 = 199495