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.

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!

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.