Revision history [back]

Hallo avicenna,

actually in occupancy grid maps like in gmapping you dont speak of (x,y) coordinates you are speaking of cells. Of course you can assume that the middlepoint of a cell are you coordinates.

So the Occoupancy Grid map is defined here but the most important is:

The map data, in row-major order, starting with (0,0). Occupancy probabilities are in the range [0,100]. Unknown is -1.

GMapping follows a maximum likelihood convention where a occupancy grid map is a Tri-State:

-1: Unknow

0: Free

100: Occupied

So what you need actually to do is iterating over the cells, check if the probability is 100 and if so print the location:

    for (int width=0; width < occupancy_grid->info.width; ++width)
    {
        for (int height=0; height < occupancy_grid->info.height; ++height)
        {
            if(occupancy_grid->data[height*occupancy_grid.info.width + width] > 0)
            {
               x = width * occupancy_grid->info.resolution + occupancy_grid->info.resolution / 2;
               y = height * occupancy_grid->info.resolution + occupancy_grid->info.resolution / 2;
            }
        }
    }

Be careful with is code! I write it out of my memory and its not optimized, with datatypes or even correct. Just an illustration how you can do it. occupancy_grid is here a pointer to a nav_msgs/OccupancyGrid Object.

The code also assumes that your (x,y)-origin is at (0,0). When its not you have to add it to x and y.