0
$\begingroup$

Rosanswers logo

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


Original comments

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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.