fixed a few bugs in gridmotiongraph, priority queue, compute shortest path, and other few places