======= Homework 05 - Path planning =======


Your task in this homework is to implement algorithms for path planning in occupancy grid.
Use provided template in {{aro_planning.zip}}.

Let's rehearse:

  * Path - an ordered set of points
  * Trajectory - Path plus velocities, accelerations etc. at each point

We will not concert ourselves with trajectory for now, just the path.
Planning will be performed on a 2D occupancy grid. 

{{:courses:aro:tutorials:occupancy_grid.png|Example occupancy grid}}

Grid is then represented as an un-oriented graph.
Some well known graph search algorithms are BFS, DFS, Dikstra and A*.
Pick the one that will give you minimal path length.

Paths close to the obstacles are infeasible, robot would collide.
Robot diameter is given.
Solution is to add a safety margin, inflate obstacles in the grid.
Use //scipy//, either approach will work:

https://docs.scipy.org/doc/scipy/reference/generated/scipy.ndimage.binary_dilation.html

https://docs.scipy.org/doc/scipy/reference/generated/scipy.ndimage.grey_dilation.html


Path planning in the occupancy grid will be implemented in ''planner.py''. 
Implement a service which takes origin and goal poses as input and returns array of poses, as is given by the ''PlanPath.srv'' from ''aro_msgs'' package. Mind that planner should be aware of occupancy grid, which will change during the map exploration.
Generated path should be safe to traverse and as short as possible. It should also contain both start and goal poses.
Assume that grid nodes are connected as a square grid (see figure).
Diagonal connections might lead to a better solution, but you will have to properly take care of corners in occupancy grid!


====== Submission and evaluation ======

Please submit the whole ''aro_planning'' package directory in a single .zip file.
Path planning will be evaluated using simulation.
Paths will be checked for length and validity.
Robot diameter will be kept the same during our testing.