vato007/breezyslam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Cleanup

Browse Source
This commit is contained in:
simondlevy
2018-06-30 15:07:37 -04:00
parent cb552472a3
commit ab151052be
1 changed files with 0 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

363
examples/Log2PGM.java
View File

@@ -1,363 +0,0 @@
/*
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.PoseChange;
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);
PoseChange poseChange = this.robot.computePoseChange(odometry[0], odometry[1], odometry[2]);
slam.update(scan, poseChange);
}
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());
}
}
}