adding last local changes to commit to clean up history

[SteveMacenski]

Adding some last bits of work from my last hybrid A* testing so that I can clean up the git history and continue