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) {