# get pose(x,y) of any unoccupied cell from 2D occupancy grid

I wish to be able to calculate the pose(x,y) of any unoccupied grid in the occupancy grid from the occupancyGrid message type here . I wish to use the map int8[ ] data which contains the probability of occupancy of each grid. I need to do so for an algorithm I'm working on. Here are several snippets on how I plan to accomplish this, please be aware that alot of the code before and inbetween have been skipped to cut out information unimportant to the focus of this question:

   ...............................
self.occ_gridmap = rospy.wait_for_message("/map", OccupancyGrid)
self.fullmap = np.array(self.occ_gridmap.data)
self.free_grid=np.where(self.fullmap==0)
self.width = self.occ_gridmap.info.width
self.height = self.occ_gridmap.info.height
self.origin_x = self.occ_gridmap.info.origin.position.x
self.origin_y = self.occ_gridmap.info.origin.position.y

...............................
def cell_grid(self, index):
e_row = index % self.height
e_column = ((index - e_row) / self.width)
return [e_row, e_column]

....................................
def pose_calculator(self,grid_location):
x=(grid_location[0]*self.resolution)+self.origin_x
y=(grid_location[1] * self.resolution) + self.origin_y
#print("x,y",[x,y])
return [x,y]

................................
def mapmaker(self):
q=[]

for i in range(len(self.free_grid[0])):
#self.cells.append(self.grid_calculator(self.free_grid[0][i]))
self.cells.append(self.cell_grid(self.free_grid[0][i]))
for i in range(len(self.cells)):
self.cells_array.append(self.pose_calculator(self.cells[i]))


As seen above self.free_grid is supposed to hold all cells with value 0(unoccupied cells) and I try to use map,resolution,origin,width and height. The problem is this doesn't seem to work. for instance, when I recalculate occupancy grid data from some of the poses (x,y) I calculated from self.free_grid, I end up with cells that do not have a value of 0. Is there a more accurate way to do this?

Originally posted by distro on ROS Answers with karma: 167 on 2022-03-08

Post score: 1

Comment by Delb on 2022-03-08:
Are you testing using a square map ? I didn't find issue but indeed when height != width the values are incorrect. You have inverted the width with the height in e_row = index % self.height.

Comment by distro on 2022-03-08:
@Delb its not a square map, in this case the occupancy grid map in focus has a width of 608 and a height of 384. with resoution of 0.05 I believe. so it should bee_row=index % self.width?

Comment by distro on 2022-03-08:
@Delb and I'm guessing it should be e_column = ((index - e_row) / self.height)? I had thought due to the int8 [] data being in row major what I was doing previously was correct.

Row major means that all your data depend on the width.

When parsing the data (an array), you start at index 0, when index = width - 1 then you have parsed through all the first row. So when index is in [0, width - 1] : row = 0 and column = index.

Now if you are in the second row then index will be in [width, 2*width - 1]: row = 1 and column = index - width

For third row index is between [2*width, 3*width-1] : row = 2 and column = index - 2*width

We see a pattern here and we can deduce the formula for row and column. The column is always the rest of index minus a multiple of the width, hence a modulo so column = index % width. Now this multiple corresponds to the row so row = (index - index % width)/width

Originally posted by Delb with karma: 3907 on 2022-03-15

This answer was ACCEPTED on the original site

Post score: 1