Dilate nav_msgs/OccupancyGrid
Hello everyone,
I am currently trying to make my own move_base node starting from a gmapping occupancy grid in a map server and AMCL localization. I wrote an A* algorithm which works perfectly but for robustness of the planning, I want the map to be dilated before being fed to A*. My robot has a radius of 0.225 meters (resolution of the occupancy grid is 0.5 m/cell). I tried several options like scipy.ndimage.binary_dilation or scipy.signal.fftconvolve but these seems not to work. I convert the nav_msgs/OccupancyGrid which I read from the "/map" topic to an python array using:
occupancy_grid = np.array(msg.data).reshape((msg.info.height, msg.info.width))
occupancy_grid[occupancy_grid > 50] = 1
occupancy_grid[occupancy_grid < 0] = 1
The grid has a 1 value for obstacles and unknown area, and 0 for free and known area. The height and width of the grid is 4000 cells (really big but generated by gmapping) So the goal is to dilate this array with the size of my robot in python. Can someone help me with this please?
Kind Regards, Matthias
