PriorityQueue tested. Need to clean up code and fix up constructor.
Merge branch 'AI' of https://github.com/UBC-Snowbots/IGVC2015 into AI
PriorityQueue tests