MCLPoseProvider.java

package lejos.robotics.localization;

import lejos.robotics.Pose;
import lejos.robotics.localization.MCLParticleSet;
import lejos.robotics.mapping.RangeMap;
import lejos.robotics.MoveListener;
import lejos.robotics.MovementProvider;
import lejos.robotics.RangeReadings;
import lejos.robotics.RangeScanner;
import lejos.robotics.Movement;

/*
 * 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.
 */

public class MCLPoseProvider implements PoseProvider, MoveListener {
	private MCLParticleSet particles;
	private RangeScanner scanner;
	private RangeMap map;
	private boolean readingsRequired = true;
	
	public MCLPoseProvider(MovementProvider mp, RangeScanner scanner, 
		                   RangeMap map, int numParticles, int border) {
		particles = new MCLParticleSet(map, numParticles, border);
		this.scanner = scanner;
		this.map = map;
		mp.addMoveListener(this);
	}
	
	public MCLParticleSet getParticles() {
		return particles;
	}

	public void movementStarted(Movement event, MovementProvider mp) {		
	}

	public void movementStopped(Movement event, MovementProvider mp) {
		readingsRequired = true;
		particles.applyMove(event);
	}

	public Pose getPose() {
		if (readingsRequired) {
			RangeReadings rr = scanner.getRangeValues();
			readingsRequired = false;
			if (!rr.incomplete()) {
				particles.calculateWeights(rr, map);
				particles.resample(); // Cannot indicate robot is lost
			}
		}
		return particles.getEstimatedPose();
	}
}