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

Restored Matlab, Java

Browse Source
This commit is contained in:
simondlevy
2018-06-30 22:36:15 -04:00
parent 16ee48385f
commit 0ac92380c5
33 changed files with 3033 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

363
examples/Log2PGM.java Normal file
View File

@@ -0,0 +1,363 @@
/*
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());
}
}
}

69
examples/MinesRover.m Executable file
View File

@@ -0,0 +1,69 @@
classdef MinesRover < WheeledRobot
%MinesRover Class for MinesRover custom robot
%
% 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/>.
properties (Access = 'private')
TICKS_PER_CYCLE = 2000;
end
methods (Access = 'private')
function degrees = ticks_to_degrees(obj, ticks)
degrees = ticks * (180. / obj.TICKS_PER_CYCLE);
end
end
methods (Access = 'public')
function robot = MinesRover()
robot = robot@WheeledRobot(77, 165);
end
function disp(obj)
% Displays information about this MinesRover
fprintf('<%s ticks_per_cycle=%d>\n', obj.str(), obj.TICKS_PER_CYCLE)
end
function [poseChange, obj] = computePoseChange(obj, odometry)
[poseChange, obj] = computePoseChange@WheeledRobot(obj, odometry(1), odometry(2), odometry(3));
end
function [timestampSeconds, leftWheelDegrees, rightWheelDegrees] = ...
extractOdometry(obj, timestamp, leftWheel, rightWheel)
% Convert microseconds to seconds
timestampSeconds = timestamp / 1e6;
% Convert ticks to angles
leftWheelDegrees = obj.ticks_to_degrees(leftWheel);
rightWheelDegrees = obj.ticks_to_degrees(rightWheel);
end
end
end

175
examples/logdemo.m Normal file
View File

@@ -0,0 +1,175 @@
%
% logdemo.m : BreezySLAM Matlab demo. Reads logfile with odometry and scan
% data from Paris Mines Tech and displays the map and robot
% position in real time.
%
% Usage:
%
% >> logdemo(dataset, [use_odometry], [random_seed])
%
% Examples:
%
% >> logdemo('exp2')
%
% For details see
%
% @inproceedings{coreslam-2010,
% author = {Bruno Steux and Oussama El Hamzaoui},
% title = {CoreSLAM: 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/>.
function logdemo(dataset, use_odometry, seed)
% Params
MAP_SIZE_PIXELS = 800;
MAP_SIZE_METERS = 32;
ROBOT_SIZE_PIXELS = 10;
% Grab input args
if nargin < 2
use_odometry = 0;
end
if nargin < 3
seed = 0;
end
% Load data from file
[times, scans, odometries] = load_data(dataset);
% Build a robot model if we want odometry
robot = [];
if use_odometry
robot = MinesRover();
end
% Create a CoreSLAM object with laser params and optional robot object
if seed
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, seed);
else
slam = Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS);
end
% Initialize previous time for delay
prevTime = 0;
% Loop over scans
for scanno = 1:size(scans, 1)
if use_odometry
% Convert odometry to velocities
[velocities,robot] = robot.computePoseChange(odometries(scanno, :));
% Update SLAM with lidar and velocities
slam = slam.update(scans(scanno,:), velocities);
else
% Update SLAM with lidar alone
slam = slam.update(scans(scanno,:));
end
% Get new position
[x_mm, y_mm, theta_degrees] = slam.getpos();
% Get current map
map = slam.getmap();
% Display map
hold off
image(map/4) % Keep bytes in [0,64] for colormap
axis('square')
colormap('gray')
hold on
% Generate a polyline to represent the robot
[x_pix, y_pix] = robot_polyline(ROBOT_SIZE_PIXELS);
% Rotate the polyline based on the robot's angular rotation
theta_radians = pi * theta_degrees / 180;
x_pix_r = x_pix * cos(theta_radians) - y_pix * sin(theta_radians);
y_pix_r = x_pix * sin(theta_radians) + y_pix * cos(theta_radians);
% Add the robot's position as offset to the polyline
x_pix = x_pix_r + mm2pix(x_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS);
y_pix = y_pix_r + mm2pix(y_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS);
% Add robot image to map
fill(x_pix, y_pix, 'r')
drawnow
% Delay based on current time in microseconds
currTime = times(scanno);
if scanno > 1
pause((currTime-prevTime)/1e6)
end
prevTime = times(scanno);
end
% Function to generate a x, y points for a polyline to display the robot
% Currently we just generate a triangle.
function [x_pix, y_pix] = robot_polyline(robot_size)
HEIGHT_RATIO = 1.25;
x_pix = [-robot_size, robot_size, -robot_size];
y_pix = [-robot_size/HEIGHT_RATIO, 0 , robot_size/HEIGHT_RATIO];
% Function to convert millimeters to pixels -------------------------------
function pix = mm2pix(mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS)
pix = floor(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS));
% Function to load all from file ------------------------------------------
% Each line in the file has the format:
%
% TIMESTAMP ... Q1 Q1 ... Distances
% (usec) (mm)
% 0 ... 2 3 ... 24 ...
%
%where Q1, Q2 are odometry values
function [times, scans,odometries] = load_data(dataset)
data = load([dataset '.dat']);
times = data(:,1);
scans = data(:,25:end-1); % avoid final ' '
odometries = data(:,[1,3,4]);
% Function to build a Laser data structure for the Hokuyo URG04LX used for
% collecting the logfile data.
function laser = MinesLaser()
laser.scan_size = 682;
laser.scan_rate_hz = 10;
laser.detection_angle_degrees = 240;
laser.distance_no_detection_mm = 4000;
laser.detection_margin = 70;
laser.offset_mm = 145;

38
examples/rpslam.py
View File

@@ -30,6 +30,9 @@ from rplidar import RPLidar as Lidar
from pltslamshow import SlamShow
from scipy.interpolate import interp1d
import numpy as np
if __name__ == '__main__':
# Connect to Lidar unit
@@ -52,36 +55,33 @@ if __name__ == '__main__':
while True:
try:
# Extrat (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extrat (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
print(angles)
except KeyboardInterrupt:
break
# Interpolate to get 360 angles from 0 through 359, and corresponding distances
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360)))
# Update SLAM with current Lidar scan, using third element of (quality, angle, distance) triples
#slam.update([item[2] for item in next(iterator)])
slam.update(distances)
# Get current robot position
#x, y, theta = slam.getpos()
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
#slam.getmap(mapbytes)
slam.getmap(mapbytes)
#display.displayMap(mapbytes)
display.displayMap(mapbytes)
#display.setPose(x, y, theta)
display.setPose(x, y, theta)
# Exit on ESCape
#key = display.refresh()
#if key != None and (key&0x1A):
# exit(0)
# Exit on window close
if not display.refresh():
exit(0)
lidar.stop()
lidar.disconnect()