[Planning] Add occupancy grid based planners

[sea-bass]

Once #1 is solved, we will have a set of planners that work directly with our polygon-based representation... which is expensive.

Given that we also have a way to generate an occupancy grid from world, we should have planning capabilities that leverage this. The ideas I have so far are:

1. Add the option for the existing planners to do collision checking using occupancy grid vs. polygons -- some timing benchmarks would be interesting
2. Add new planners that require the occupancy grid -- for example, grid-based A*

Some good ideas might be to look into Peter Corke's Robotics Toolbox for Python and/or Atsushi Sakai's PythonRobotics package. I would be thrilled to have either or both as dependencies for the planners.

[ibrahiminfinite]

Starting a discussion here to finalize on an implementation before proceeding with PR #70 .

The current considerations are:

1. Should occupancy grid be a part of the planner or the world class. ?
2. How to make the the world.check_occupancy() method aware of the occupancy grid and if to use it.
3. Occupancy Grid should have a method to check occupancy of a cell.
4. Should the world to grid coordinate conversion happen implicitly in the is_occupied() method or explicitly convert and be passed in?.

[ibrahiminfinite]

@sea-bass , my thoughts on the above points.

• OccupancyGrid should be a part of the planner
• World to Grid coordinate conversion should take place implicitly since we are not using the conversion anywhere else

Some ideas for implementation.

• We add 2 attributes to the planner , use_grid: bool = False and occupancy_grid: OccupancyGrid = None
• An occupany_grid parameter for world.sample_free_robo_pose_uniform() with default None
• An occupany_grid parameter for world.check_occupancy() with default None

• If use_grid is True then during construction the planner will generate an OccupancyGrid with given specifications and assign it to occupancy_grid. Then during the planning (sampling) phase, the sample_configuration() method of the planner will check self.use_grid . If True then the world.sample_free_robot_pose_uniform() is called with occupancy_grid set to the planners occupancy grid.

The world.sample_free_robot_pose_unifrom() method will in turn call the world.check_occupancy() with the occupancy_grid param that it received. In the world.check_occupancy(), if occupancy_grid is not None we use the is_occupied() method of the grid. otherwise we go for the polygon based approach.

[sea-bass]

This seems good, but the key idea behind an occupancy grid based planner is NOT sampling.

Note that item 1 in the original issue only talks about using the occupancy grid for collision checking, but not for planning itself.

So besides the above, just directly solving A* over the entire grid from start to goal would be a first step towards this. In other words, creating a new planner.

[ibrahiminfinite]

The points I mentioned above was only aimed at solving point number 1. Since currently every point sampled makes num_rooms + num_hallways checks in the worst case. and the occupancy grid would reduce it to just one check per sample. Should I add the above mentioned solution for point 1 first and then move on to 2?

[ibrahiminfinite]

Now that I take a look at the planners again. the check_occupancy is also called from inside SearchGraph.check_connectivity( ) this would mean we have to pass around the use_grid parameter in more places. Might be better to drop point 1 and just make separate grid based planners.

[sea-bass]

Hah, yeah, good point. The connectivity check for a straight-line in an occupancy grid is a ray cast and that is a bit annoying.

I for sure would like to see point 2 more than point 1, if that's where you would like to focus efforts.

[ibrahiminfinite]

Will proceed with a grid based A-star to get things started.