ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

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

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?

edit retag reopen merge delete

### Closed for the following reason the question is answered, right answer was accepted by distro close date 2023-01-16 12:11:01.858604

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.

( 2022-03-08 07:50:19 -0500 )edit

@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?

( 2022-03-08 14:52:01 -0500 )edit

@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.

( 2022-03-08 14:54:40 -0500 )edit

Sort by ยป oldest newest most voted

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

more