AStar is mostly done, just need to implement communication with the local navigator. Major problems running system now...