-Checkin the current status of the AStar planner.  Discretization of the space is complete, and AStar is implemented and working.  I just need to generate footsteps based on the 2D path