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?
Are you testing using a square map ? I didn't find issue but indeed when
height != width
the values are incorrect. You have inverted thewidth
with theheight
ine_row = index % self.height
.@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 be
e_row=index % self.width
?@Delb and I'm guessing it should be
e_column = ((index - e_row) / self.height)
? I had thought due to theint8 [] data
being in row major what I was doing previously was correct.