package lejos.robotics.proposal;
import java.awt.Rectangle;
import java.util.ArrayList;
import java.util.Collection;
import lejos.geom.Point;
import lejos.robotics.Pose;
import lejos.robotics.RangeReading;
import lejos.robotics.RangeReadings;
import lejos.robotics.mapping.RangeMap;
/*
* WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
* DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
*/
/**
* PathFinder that takes a map and a dummy set of range readings.
* It finds a path that is in short moves, has no obstacles in the
* way and where valid range readings can be taken from each waypoint.
*
* The algorithm is not deterministic so each time it is called a new route
* will be found.
*
* @author Lawrie Griffiths
*
*/
public class MapPathFinder extends ArrayList<WayPoint> implements PathFinder {
private static final long serialVersionUID = 1L;
private static final int MAX_ITERATIONS = 1000;
private static final float MAX_DISTANCE = 40;
private static final float MIN_GAIN = 10;
private static final float MAX_RANGE = 100;
private static int BORDER = 20;
private RangeMap map;;
private RangeReadings readings;
public MapPathFinder(RangeMap map, RangeReadings readings) {
this.map = map;
this.readings = readings;
}
public Collection<WayPoint> findRoute(Pose start, Point destination)
throws DestinationUnreachableException {
// TODO Auto-generated method stub
return null;
}
public Collection<WayPoint> findRoute(Pose start, Pose destination)
throws DestinationUnreachableException {
Pose pose = start;
// Continue until we return a route or throw DestinationUnReachableException
for(;;) {
// If the current pose if close enough to the destination, go straight there
if (pose.distanceTo(destination.getLocation()) < MAX_DISTANCE) {
add(new WayPoint(destination));
return this;
} else {
Pose testPose = null;
// Generate random poses and apply tests to them
for(int i=0;i<MAX_ITERATIONS;i++) {
testPose = generatePose();
// The new Pose must not be more than MAX_DISTANCE away from current pose
if (testPose.distanceTo(pose.getLocation()) > MAX_DISTANCE) continue;
// The new pose must be at least MIN_GAIN closer to the destination
if (pose.distanceTo(destination.getLocation()) -
testPose.distanceTo(destination.getLocation()) < MIN_GAIN)
continue;
// We must be able to get a valid set of range readings from the new pose
float heading = testPose.getHeading();
boolean validReadings = true;
for(RangeReading r: readings) {
testPose.setHeading(heading + r.getAngle());
float range = map.range(testPose);
if (range > MAX_RANGE) {
validReadings = false;
break;
}
}
if (!validReadings) continue;
//Check there are no obstacles in the way
testPose.setHeading(testPose.angleTo(pose.getLocation()));
if (map.range(testPose) < testPose.distanceTo(pose.getLocation()))
continue;
testPose.setHeading(heading); // Restore heading
break; // We have a new way point
}
if (testPose == null) throw new DestinationUnreachableException();
else {
add(new WayPoint(testPose));
pose = testPose;
}
}
}
}
/**
* Generate a random pose within the mapped area, not too close to the edge
*/
private Pose generatePose() {
float x, y, heading;
Rectangle boundingRect = map.getBoundingRect();
Rectangle innerRect = new Rectangle(boundingRect.x + BORDER, boundingRect.y + BORDER,
boundingRect.width - BORDER * 2, boundingRect.height - BORDER * 2);
// Generate x, y values in bounding rectangle
for (;;) { // infinite loop that we break out of when we have
// generated a particle within the mapped area
x = innerRect.x + (((float) Math.random()) * innerRect.width);
y = innerRect.y + (((float) Math.random()) * innerRect.height);
if (map.inside(new Point(x, y))) break;
}
// Pick a random angle
heading = ((float) Math.random()) * 360;
return new Pose(x,y,heading);
}
}