Pose start = localize();
// Find a route home
Pose home = new Pose(50, 300, -90);
PathFinder pf = new MapPathFinder(map, readings);
PoseController pc = new ArcPoseController(pilot, mcl);
System.out.println("Located: (" + (int) start.getX() + "," + (int) start.getY() + ")");
// Go home
try {
Collection<WayPoint> route = pf.findRoute(start, home);
for(WayPoint wp: route) {
System.out.println("Go to (" + (int) wp.x + "," + (int) wp.y + ")");
Pose pose = pc.goTo(wp);
goodEstimate(pose); // Just for diagnostics
// Pose controller should have a goTo(Pose) method to do this
pilot.rotate(wp.getHeading() - pose.getHeading());
}
} catch (DestinationUnreachableException e) {