First implementation of A* Planner.  This is not for real obstacles.  Very simple implementation.