首先載入一張地圖作為示例

rosrun map_server map_server xxx.yaml 

可以得到如下圖的地圖資訊：

在對應的yaml檔案中，儲存的資訊為：
image: a314.pgm
resolution: 0.050000
origin: [-20.000000, -20.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

我們首先看一下與地圖相關的訊息：

地圖元資訊：
nav_msgs/MapMetaData Message

# This hold basic information about the characterists of the OccupancyGrid
# The time at which the map was loaded
time map_load_time
# The map resolution [m/cell]
float32 resolution
# Map width [cells]
uint32 width
# Map height [cells]
uint32 height
# The origin of the map [m, m, rad].  This is the real-world pose of the
# cell (0,0) in the map.
geometry_msgs/Pose origin
 

佔據柵格資訊
nav_msgs/OccupancyGrid Message

# This represents a 2-D grid map, in which each cell represents the probability of  #概率柵格
# occupancy.
Header header 
#MetaData for the map
MapMetaData info  #包含地圖元資料資訊
# The map data, in row-major order, starting with (0,0).  Occupancy     #以行的順序儲存
# probabilities are in the range [0,100].  Unknown is -1.
int8[] data
 

這裡需要注意的是：

geometry_msgs/Pose origin
The origin of the map [m, m, rad]. This is the real-world pose of the cell (0,0) in the map.
即為左下角 畫素在地圖座標系中的座標，在畫素座標系中，左下角為畫素（0,0）的座標

通過佔據柵格資訊資訊中的 int8[] data ，我們知道，data為一個向量，這個向量的大小為柵格的寬度×柵格的高度，柵格地圖的設定方法如下：

   nav_msgs::OccupancyGrid map_;
   map_.header.frame_id = "map";
    map_.info.height =400; //假設高度為400個畫素
    map_.info.width =200;// 寬度為200個畫素
    map_.info.resolution = 0.05; // 解析度為5cm/pixel
 

設定的結果如下：

那麼如何讓地圖對稱呢？這就需要設定左下角畫素在地圖座標系中的座標

 map_.info.origin.position.x = - map_.info.width* map_.info.resolution;   // 這裡為整數除法！所以都是以畫素的左下角為座標原點
    map_.info.origin.position.y =- map_.info.height* map_.info.resolution;
    map_.info.origin.orientation.x=0;
    map_.info.origin.orientation.y=0;
    map_.info.origin.orientation.z=0;
    map_.info.origin.orientation.w = 1.0;

效果如下：

map_.info.origin.position.x = -200/2* map_.info.resolution;
map_.info.origin.position.y =- 400/2* map_.info.resolution;
map_.info.origin.orientation.x=0;
map_.info.origin.orientation.y=0;
map_.info.origin.orientation.z=0;
map_.info.origin.orientation.w = 1.0;
map_.data.assign( 200*400, -1); // 對所有柵格賦值，-1為未知

接下來的問題是如何修改某一個柵格的值。
通過上述可知，柵格地圖的儲存結構為一個向量，向量的大小為寬度*高度
這些柵格是從左下角為0，以行主的順序，逐漸增加。
簡單地說，左下角的索引為0，左下角右側的柵格索引為1，第200個柵格為左下角北側的畫素，
這裡從0開始，逐漸遍歷每個柵格，效果如下圖：

則要設定（1，0）柵格的方法為

index = 1*200 + 0

設定(i,j)柵格的方法為：

index = j* width + i;

好，鋪墊完成，我們開始主題，如何獲取地圖座標系中的某一個點的畫素座標。

首先，由於左下角畫素相對於地圖座標系的座標為：

map_.info.origin.position.x = -200/2* map_.info.resolution;
map_.info.origin.position.y =- 400/2* map_.info.resolution;
則地圖座標系相對於左下角的畫素座標為

index = 200* 200 +100 

以地圖的畫素座標為座標原點，假設機器人的位置為(1,0.23)
則，機器人所在的地影象素座標系的柵格為：
1/解析度 = 1/0.05 =20
0.23/解析度 = 0.23/0.05 = 4.6
也就是在第20個和第4個柵格中,那麼只需要把地影象素座標和機器人在地影象素座標中的值相加即可：
(100+20,200+4)

index  =204*200+120

畫出map所在的柵格和機器人所在的柵格效果圖如下：

整個程式碼如下，需要釋出鐳射話題才可以顯示地圖原點

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <iostream>
using namespace std;
int i=0;
int w=4;
int h =3;
ros::Publisher map_publisher;
nav_msgs::OccupancyGrid map_;
void handleLaserScan(sensor_msgs::LaserScan msg)
{

   map_.data.at(w*h/2+w/2) =0;
    map_publisher.publish(map_);

}

void initMap();
int main(int argc, char **argv)
{
  ros::init(argc, argv, "local_map");
  ros::NodeHandle nh;
  initMap();


  ros::Subscriber scanHandler = nh.subscribe<sensor_msgs::LaserScan>("scan", 1, handleLaserScan);

  map_publisher = nh.advertise<nav_msgs::OccupancyGrid>("local_map", 1, true);

  ros::spin();
}

void initMap()
{
    map_.header.frame_id = "map";
    map_.info.height =h;
    map_.info.width = w;
    map_.info.resolution = 1;
    map_.info.origin.position.x = -w/2* map_.info.resolution; 
    map_.info.origin.position.y =- h/2* map_.info.resolution;

    map_.info.origin.orientation.x=0;
    map_.info.origin.orientation.y=0;
    map_.info.origin.orientation.z=0;
    map_.info.origin.orientation.w = 1.0;
    map_.data.assign( w*h, -1);

}