Added cost_function and fixed some errors

1. cost_function takes in graph, ids of two nodes and (possibly
unnecessary) observed_map.
2. GridMotionGraph.computePred needs to change a bit because of my
cost_function
3. Fixed some errors in D*, ComputeShortestPath and UpdateVertex
4. Added isEmpty function to PriorityQueue (doesn't really matter
though)