Pathing: Optimize Optimize Optimize!

[mistrynick]

in kestrel_planning

1. remove redundant costmap lookups, ex (getSuccessors())
2. Fix heuristics and also repeated calculations in computeShortestPath
3. only reset visited states in initialize()
4. in getSuccessors(), instead of a triple for loop, precompute the directions
5. in topOpenList, check if the keys are still valid rather than computing key code again and again