Your task in this homework is to implement algorithms for path planning in occupancy grid. Use provided template in hw5.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. Grid nodes can be connected as a square grid (see figure) or a 8-neighborhood. Diagonal connections will lead to a better solution, but you will have to properly take care of corners in occupancy grid!

The provided package contains planner.launch file, which enables data visualization in RViz. To obtain published OccupancyGrid on the /occupancy topic, you can execute rosbag play -l occ_dat.bag from the aro_planning/config directory.

The service can be called using the command line:

rosservice call /plan_path "start:
  x: 0.0
  y: 0.0
  theta: 0.0
goal:
  x: -1.0
  y: 0.0
  theta: 0.0"

Although debugging your code through Brute submission system might work, as the evaluation procedure is very simple, learning to visualize your data and debugging locally will help you with the semestral work down the line.

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.