364 lines
12 KiB
Java
364 lines
12 KiB
Java
/*
|
|
Log2PGM.java : BreezySLAM demo. Reads logfile with odometry and scan data from
|
|
Paris Mines Tech and produces a .PGM image file showing robot path
|
|
and final map.
|
|
|
|
For details see
|
|
|
|
@inproceedings{,
|
|
author = {Bruno Steux and Oussama El Hamzaoui},
|
|
title = {SinglePositionSLAM: a SLAM Algorithm in less than 200 lines of C code},
|
|
booktitle = {11th International Conference on Control, Automation, Robotics and Vision, ICARCV 2010, Singapore, 7-10
|
|
December 2010, Proceedings},
|
|
pages = {1975-1979},
|
|
publisher = {IEEE},
|
|
year = {2010}
|
|
}
|
|
|
|
Copyright (C) 2014 Simon D. Levy
|
|
|
|
This code is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU Lesser General Public License as
|
|
published by the Free Software Foundation, either version 3 of the
|
|
License, or (at your option) any later version.
|
|
|
|
This code is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public License
|
|
along with this code. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
import edu.wlu.cs.levy.breezyslam.components.Map;
|
|
import edu.wlu.cs.levy.breezyslam.components.Scan;
|
|
import edu.wlu.cs.levy.breezyslam.components.Position;
|
|
import edu.wlu.cs.levy.breezyslam.components.URG04LX;
|
|
import edu.wlu.cs.levy.breezyslam.components.Velocities;
|
|
|
|
import edu.wlu.cs.levy.breezyslam.robots.WheeledRobot;
|
|
|
|
import edu.wlu.cs.levy.breezyslam.algorithms.RMHCSLAM;
|
|
import edu.wlu.cs.levy.breezyslam.algorithms.DeterministicSLAM;
|
|
import edu.wlu.cs.levy.breezyslam.algorithms.SinglePositionSLAM;
|
|
|
|
import java.io.BufferedReader;
|
|
import java.io.FileReader;
|
|
import java.io.BufferedWriter;
|
|
import java.io.FileWriter;
|
|
import java.io.IOException;
|
|
import java.util.Vector;
|
|
|
|
public class Log2PGM
|
|
{
|
|
private static int MAP_SIZE_PIXELS = 800;
|
|
private static double MAP_SIZE_METERS = 32;
|
|
private static int SCAN_SIZE = 682;
|
|
|
|
private SinglePositionSLAM slam;
|
|
|
|
private boolean use_odometry;
|
|
private int random_seed;
|
|
private Rover robot;
|
|
|
|
static int coords2index(double x, double y)
|
|
{
|
|
return (int)(y * MAP_SIZE_PIXELS + x);
|
|
}
|
|
|
|
int mm2pix(double mm)
|
|
{
|
|
return (int)(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS));
|
|
}
|
|
|
|
|
|
// Class for Mines verison of URG-04LX Lidar -----------------------------------
|
|
|
|
private class MinesURG04LX extends URG04LX
|
|
{
|
|
public MinesURG04LX()
|
|
{
|
|
super( 70, // detectionMargin
|
|
145); // offsetMillimeters
|
|
}
|
|
}
|
|
|
|
// Class for MinesRover custom robot -------------------------------------------
|
|
|
|
private class Rover extends WheeledRobot
|
|
{
|
|
public Rover()
|
|
{
|
|
super(77, // wheelRadiusMillimeters
|
|
165); // halfAxleLengthMillimeters
|
|
}
|
|
|
|
protected WheeledRobot.WheelOdometry extractOdometry(
|
|
double timestamp,
|
|
double leftWheelOdometry,
|
|
double rightWheelOdometry)
|
|
{
|
|
// Convert microseconds to seconds, ticks to angles
|
|
return new WheeledRobot.WheelOdometry(
|
|
timestamp / 1e6,
|
|
ticksToDegrees(leftWheelOdometry),
|
|
ticksToDegrees(rightWheelOdometry));
|
|
}
|
|
|
|
protected String descriptorString()
|
|
{
|
|
return String.format("ticks_per_cycle=%d", this.TICKS_PER_CYCLE);
|
|
}
|
|
|
|
|
|
private double ticksToDegrees(double ticks)
|
|
{
|
|
return ticks * (180. / this.TICKS_PER_CYCLE);
|
|
}
|
|
|
|
private int TICKS_PER_CYCLE = 2000;
|
|
}
|
|
|
|
// Progress-bar class ----------------------------------------------------------------
|
|
// Adapted from http://code.activestate.com/recipes/168639-progress-bar-class/
|
|
// Downloaded 12 January 2014
|
|
|
|
private class ProgressBar
|
|
{
|
|
public ProgressBar(int minValue, int maxValue, int totalWidth)
|
|
{
|
|
this.progBar = "[]";
|
|
this.min = minValue;
|
|
this.max = maxValue;
|
|
this.span = maxValue - minValue;
|
|
this.width = totalWidth;
|
|
this.amount = 0; // When amount == max, we are 100% done
|
|
this.updateAmount(0); // Build progress bar string
|
|
}
|
|
|
|
void updateAmount(int newAmount)
|
|
{
|
|
if (newAmount < this.min)
|
|
{
|
|
newAmount = this.min;
|
|
}
|
|
if (newAmount > this.max)
|
|
{
|
|
newAmount = this.max;
|
|
}
|
|
|
|
this.amount = newAmount;
|
|
|
|
// Figure out the new percent done, round to an integer
|
|
float diffFromMin = (float)(this.amount - this.min);
|
|
int percentDone = (int)java.lang.Math.round((diffFromMin / (float)this.span) * 100.0);
|
|
|
|
// Figure out how many hash bars the percentage should be
|
|
int allFull = this.width - 2;
|
|
int numHashes = (int)java.lang.Math.round((percentDone / 100.0) * allFull);
|
|
|
|
// Build a progress bar with hashes and spaces
|
|
this.progBar = "[";
|
|
this.addToProgBar("#", numHashes);
|
|
this.addToProgBar(" ", allFull-numHashes);
|
|
this.progBar += "]";
|
|
|
|
// Figure out where to put the percentage, roughly centered
|
|
int percentPlace = (this.progBar.length() / 2) - ((int)(java.lang.Math.log10(percentDone+1)) + 1);
|
|
String percentString = String.format("%d%%", percentDone);
|
|
|
|
// Put it there
|
|
this.progBar = this.progBar.substring(0,percentPlace) + percentString + this.progBar.substring(percentPlace+percentString.length());
|
|
}
|
|
|
|
String str()
|
|
{
|
|
return this.progBar;
|
|
}
|
|
|
|
private String progBar;
|
|
private int min;
|
|
private int max;
|
|
private int span;
|
|
private int width;
|
|
private int amount;
|
|
|
|
private void addToProgBar(String s, int n)
|
|
{
|
|
for (int k=0; k<n; ++k)
|
|
{
|
|
this.progBar += s;
|
|
}
|
|
}
|
|
}
|
|
|
|
public Log2PGM(boolean use_odometry, int random_seed)
|
|
{
|
|
MinesURG04LX laser = new MinesURG04LX();
|
|
|
|
this.robot = new Rover();
|
|
|
|
this.slam = random_seed > 0 ?
|
|
new RMHCSLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed) :
|
|
new DeterministicSLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS);
|
|
|
|
this.use_odometry = use_odometry;
|
|
this.random_seed = random_seed;
|
|
}
|
|
|
|
public byte [] processData(Vector<int []> scans, Vector<long []> odometries)
|
|
{
|
|
// Report what we're doing
|
|
int nscans = scans.size();
|
|
ProgressBar progbar = new ProgressBar(0, nscans, 80);
|
|
|
|
System.out.printf("Processing %d scans with%s odometry / with%s particle filter...\n",
|
|
nscans, this.use_odometry ? "" : "out", this.random_seed > 0 ? "" : "out");
|
|
|
|
Vector<double []> trajectory = new Vector<double []>();
|
|
|
|
for (int scanno=0; scanno<nscans; ++scanno)
|
|
{
|
|
int [] scan = scans.elementAt(scanno);
|
|
|
|
// Update with/out odometry
|
|
if (this.use_odometry)
|
|
{
|
|
long [] odometry = odometries.elementAt(scanno);
|
|
Velocities velocities = this.robot.computeVelocities(odometry[0], odometry[1], odometry[2]);
|
|
slam.update(scan, velocities);
|
|
}
|
|
else
|
|
{
|
|
slam.update(scan);
|
|
}
|
|
|
|
Position position = slam.getpos();
|
|
|
|
// Add new coordinates to trajectory
|
|
double [] v = new double[2];
|
|
v[0] = position.x_mm;
|
|
v[1] = position.y_mm;
|
|
trajectory.add(v);
|
|
|
|
|
|
progbar.updateAmount(scanno);
|
|
System.out.printf("\r%s", progbar.str());
|
|
}
|
|
|
|
// Get final map
|
|
byte [] mapbytes = new byte [MAP_SIZE_PIXELS*MAP_SIZE_PIXELS];
|
|
slam.getmap(mapbytes);
|
|
|
|
// Put trajectory into map as black pixels
|
|
for (int k=0; k<trajectory.size(); ++k)
|
|
{
|
|
double [] v = trajectory.elementAt(k);
|
|
|
|
int x = mm2pix(v[0]);
|
|
int y = mm2pix(v[1]);
|
|
|
|
mapbytes[coords2index(x, y)] = 0;
|
|
}
|
|
|
|
return mapbytes;
|
|
}
|
|
|
|
// main -------------------------------------------------------------------------------------------------------------
|
|
|
|
public static void main(String[] argv)
|
|
{
|
|
// Bozo filter for input args
|
|
if (argv.length < 3)
|
|
{
|
|
System.err.println("Usage: java log2pgm <dataset> <use_odometry> <random_seed>");
|
|
System.err.println("Example: java log2pgm exp2 1 9999");
|
|
System.exit(1);
|
|
}
|
|
|
|
// Grab input args
|
|
String dataset = argv[0];
|
|
boolean use_odometry = Integer.parseInt(argv[1]) == 0 ? false : true;
|
|
int random_seed = argv.length > 2 ? Integer.parseInt(argv[2]) : 0;
|
|
|
|
// Load the data from the file
|
|
|
|
Vector<int []> scans = new Vector<int []>();
|
|
Vector<long []> odometries = new Vector<long []>();;
|
|
|
|
String filename = dataset + ".dat";
|
|
System.out.printf("Loading data from %s ... \n", filename);
|
|
|
|
BufferedReader input = null;
|
|
|
|
try
|
|
{
|
|
FileReader fstream = new FileReader(filename);
|
|
input = new BufferedReader(fstream);
|
|
while (true)
|
|
{
|
|
String line = input.readLine();
|
|
if (line == null)
|
|
{
|
|
break;
|
|
}
|
|
|
|
String [] toks = line.split(" +");
|
|
|
|
long [] odometry = new long [3];
|
|
odometry[0] = Long.parseLong(toks[0]);
|
|
odometry[1] = Long.parseLong(toks[2]);
|
|
odometry[2] = Long.parseLong(toks[3]);
|
|
odometries.add(odometry);
|
|
|
|
int [] scan = new int [SCAN_SIZE];
|
|
for (int k=0; k<SCAN_SIZE; ++k)
|
|
{
|
|
scan[k] = Integer.parseInt(toks[k+24]);
|
|
}
|
|
scans.add(scan);
|
|
}
|
|
input.close();
|
|
}
|
|
catch (IOException e)
|
|
{
|
|
System.err.println("Error: " + e.getMessage());
|
|
}
|
|
|
|
// Create a Log2PGM object to process the data
|
|
Log2PGM log2pgm = new Log2PGM(use_odometry, random_seed);
|
|
|
|
// Process the scan and odometry data, returning a map
|
|
byte [] mapbytes = log2pgm.processData(scans, odometries);
|
|
|
|
// Save map and trajectory as PGM file
|
|
|
|
filename = dataset + ".pgm";
|
|
System.out.println("\nSaving map to file " + filename);
|
|
|
|
BufferedWriter output = null;
|
|
|
|
try
|
|
{
|
|
FileWriter fstream = new FileWriter(filename);
|
|
output = new BufferedWriter(fstream);
|
|
output.write(String.format("P2\n%d %d 255\n", MAP_SIZE_PIXELS, MAP_SIZE_PIXELS));
|
|
for (int y=0; y<MAP_SIZE_PIXELS; y++)
|
|
{
|
|
for (int x=0; x<MAP_SIZE_PIXELS; x++)
|
|
{
|
|
// Output unsigned byte value
|
|
output.write(String.format("%d ", (int)mapbytes[coords2index(x, y)] & 0xFF));
|
|
}
|
|
output.write("\n");
|
|
}
|
|
output.close();
|
|
}
|
|
catch (IOException e)
|
|
{
|
|
System.err.println("Error: " + e.getMessage());
|
|
}
|
|
}
|
|
}
|