/**
* * Move the robot and update the distribution with the action model
*
* @param distance
* @param _heading
* @param _sensorModel
*/
private void move(
float distance, Heading _heading, ActionModel _actionModel, SensorModel _sensorModel) {
// move robot
m_robot.translate(m_translationAmount);
// now update estimate of position using the action model
m_distribution = _actionModel.updateAfterMove(m_distribution, _heading);
// if visualising, update the shown distribution
if (m_mapVis != null) {
m_mapVis.setDistribution(m_distribution);
}
// A short delay so we can see what's going on
Delay.msDelay(1000);
m_distribution =
_sensorModel.updateAfterSensing(m_distribution, _heading, m_robot.getRangeValues());
// if visualising, update the shown distribution
if (m_mapVis != null) {
m_mapVis.setDistribution(m_distribution);
}
// A short delay so we can see what's going on
Delay.msDelay(1000);
}
/**
* Optionally run the visualisation of the robot and localisation process. This is not necessary
* to run the localisation and could be removed once on the real robot.
*/
public void visualise() {
JFrame frame = new JFrame("Map Viewer");
frame.addWindowListener(new KillMeNow());
// visualise the distribution on top of a line map
m_mapVis = new GridPositionDistributionVisualisation(m_distribution, m_lineMap, 2);
// Visualise the robot
m_mapVis.addRobot(m_robot);
frame.add(m_mapVis);
frame.pack();
frame.setSize(1050, 600);
frame.setVisible(true);
}