AStar and GoalChooser, done but having trouble with an inperfect OccupancyMap.