ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

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

asked 2022-03-08 02:21:04 -0500

distro gravatar image

updated 2022-03-08 03:29:47 -0500

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 flag offensive 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

Comments

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.

Delb gravatar image Delb  ( 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?

distro gravatar image distro  ( 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.

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

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-03-15 09:15:01 -0500

Delb gravatar image

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

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-03-08 02:21:04 -0500

Seen: 427 times

Last updated: Mar 15 '22