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

Removed support for Matlab, Java.

Browse Source
This commit is contained in:
simondlevy
2018-06-30 15:01:47 -04:00
parent e16a904e12
commit 6557a0af48
33 changed files with 8 additions and 3135 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
examples/MinesRover.m
View File

@@ -1,69 +0,0 @@
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

439
examples/log2pgm.cpp
View File

@@ -1,439 +0,0 @@
/*
log2pgm.cpp : 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/>.
Change log:
20-APR-2014 - Simon D. Levy - Get params from command line
05-JUN-2014 - SDL - get random seed from command line
*/
// SinglePositionSLAM params: gives us a nice-size map
static const int MAP_SIZE_PIXELS = 800;
static const double MAP_SIZE_METERS = 32;
static const int SCAN_SIZE = 682;
// Arbitrary maximum length of line in input logfile
#define MAXLINE 10000
#include <iostream>
#include <vector>
using namespace std;
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include "Position.hpp"
#include "Laser.hpp"
#include "WheeledRobot.hpp"
#include "PoseChange.hpp"
#include "algorithms.hpp"
// Methods to load all data 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
static void skiptok(char ** cpp)
{
*cpp = strtok(NULL, " ");
}
static int nextint(char ** cpp)
{
skiptok(cpp);
return atoi(*cpp);
}
static void load_data(
const char * dataset,
vector<int *> & scans,
vector<long *> & odometries)
{
char filename[256];
sprintf(filename, "%s.dat", dataset);
printf("Loading data from %s ... \n", filename);
FILE * fp = fopen(filename, "rt");
if (!fp)
{
fprintf(stderr, "Failed to open file\n");
exit(1);
}
char s[MAXLINE];
while (fgets(s, MAXLINE, fp))
{
char * cp = strtok(s, " ");
long * odometry = new long [3];
odometry[0] = atol(cp);
skiptok(&cp);
odometry[1] = nextint(&cp);
odometry[2] = nextint(&cp);
odometries.push_back(odometry);
// Skip unused fields
for (int k=0; k<20; ++k)
{
skiptok(&cp);
}
int * scanvals = new int [SCAN_SIZE];
for (int k=0; k<SCAN_SIZE; ++k)
{
scanvals[k] = nextint(&cp);
}
scans.push_back(scanvals);
}
fclose(fp);
}
// Class for Mines verison of URG-04LX Lidar -----------------------------------
class MinesURG04LX : public URG04LX
{
public:
MinesURG04LX(void): URG04LX(
70, // detectionMargin
145) // offsetMillimeters
{
}
};
// Class for MinesRover custom robot -------------------------------------------
class Rover : WheeledRobot
{
public:
Rover() : WheeledRobot(
77, // wheelRadiusMillimeters
165) // halfAxleLengthMillimeters
{
}
PoseChange computePoseChange(long * odometry, PoseChange & poseChange)
{
return WheeledRobot::computePoseChange(
odometry[0],
odometry[1],
odometry[2]);
}
protected:
void extractOdometry(
double timestamp,
double leftWheelOdometry,
double rightWheelOdometry,
double & timestampSeconds,
double & leftWheelDegrees,
double & rightWheelDegrees)
{
// Convert microseconds to seconds, ticks to angles
timestampSeconds = timestamp / 1e6;
leftWheelDegrees = ticksToDegrees(leftWheelOdometry);
rightWheelDegrees = ticksToDegrees(rightWheelOdometry);
}
void descriptorString(char * str)
{
sprintf(str, "ticks_per_cycle=%d", this->TICKS_PER_CYCLE);
}
private:
double ticksToDegrees(double ticks)
{
return ticks * (180. / this->TICKS_PER_CYCLE);
}
static const int TICKS_PER_CYCLE = 2000;
};
// Progress-bar class
// Adapted from http://code.activestate.com/recipes/168639-progress-bar-class/
// Downloaded 12 January 2014
class ProgressBar
{
public:
ProgressBar(int minValue, int maxValue, int totalWidth)
{
strcpy(this->progBar, "[]"); // This holds the progress bar string
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)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)round((percentDone / 100.0) * allFull);
// Build a progress bar with hashes and spaces
strcpy(this->progBar, "[");
this->addToProgBar("#", numHashes);
this->addToProgBar(" ", allFull-numHashes);
strcat(this->progBar, "]");
// Figure out where to put the percentage, roughly centered
int percentPlace = (strlen(this->progBar) / 2) - ((int)(log10(percentDone+1)) + 1);
char percentString[5];
sprintf(percentString, "%d%%", percentDone);
// Put it there
for (int k=0; k<strlen(percentString); ++k)
{
this->progBar[percentPlace+k] = percentString[k];
}
}
char * str()
{
return this->progBar;
}
private:
char progBar[1000]; // more than we should ever need
int min;
int max;
int span;
int width;
int amount;
void addToProgBar(const char * s, int n)
{
for (int k=0; k<n; ++k)
{
strcat(this->progBar, s);
}
}
};
// Helpers ----------------------------------------------------------------
int coords2index(double x, double y)
{
return y * MAP_SIZE_PIXELS + x;
}
int mm2pix(double mm)
{
return (int)(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS));
}
int main( int argc, const char** argv )
{
// Bozo filter for input args
if (argc < 3)
{
fprintf(stderr,
"Usage: %s <dataset> <use_odometry> <random_seed>\n",
argv[0]);
fprintf(stderr, "Example: %s exp2 1 9999\n", argv[0]);
exit(1);
}
// Grab input args
const char * dataset = argv[1];
bool use_odometry = atoi(argv[2]) ? true : false;
int random_seed = argc > 3 ? atoi(argv[3]) : 0;
// Load the Lidar and odometry data from the file
vector<int *> scans;
vector<long *> odometries;
load_data(dataset, scans, odometries);
// Build a robot model in case we want odometry
Rover robot = Rover();
// Create a byte array to receive the computed maps
unsigned char * mapbytes = new unsigned char[MAP_SIZE_PIXELS * MAP_SIZE_PIXELS];
// Create SLAM object
MinesURG04LX laser;
SinglePositionSLAM * slam = random_seed ?
(SinglePositionSLAM*)new RMHC_SLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed) :
(SinglePositionSLAM*)new Deterministic_SLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS);
// Report what we're doing
int nscans = scans.size();
printf("Processing %d scans with%s odometry / with%s particle filter...\n",
nscans, use_odometry ? "" : "out", random_seed ? "" : "out");
ProgressBar * progbar = new ProgressBar(0, nscans, 80);
// Start with an empty trajectory of positions
vector<double *> trajectory;
// Start timing
time_t start_sec = time(NULL);
// Loop over scans
for (int scanno=0; scanno<nscans; ++scanno)
{
int * lidar = scans[scanno];
// Update with/out odometry
if (use_odometry)
{
PoseChange poseChange = robot.computePoseChange(odometries[scanno], poseChange);
slam->update(lidar, poseChange);
}
else
{
slam->update(lidar);
}
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.push_back(v);
// Tame impatience
progbar->updateAmount(scanno);
printf("\r%s", progbar->str());
fflush(stdout);
}
// Report speed
time_t elapsed_sec = time(NULL) - start_sec;
printf("\n%d scans in %ld seconds = %f scans / sec\n",
nscans, elapsed_sec, (float)nscans/elapsed_sec);
// Get final map
slam->getmap(mapbytes);
// Put trajectory into map as black pixels
for (int k=0; k<(int)trajectory.size(); ++k)
{
double * v = trajectory[k];
int x = mm2pix(v[0]);
int y = mm2pix(v[1]);
delete v;
mapbytes[coords2index(x, y)] = 0;
}
// Save map and trajectory as PGM file
char filename[100];
sprintf(filename, "%s.pgm", dataset);
printf("\nSaving map to file %s\n", filename);
FILE * output = fopen(filename, "wt");
fprintf(output, "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++)
{
fprintf(output, "%d ", mapbytes[coords2index(x, y)]);
}
fprintf(output, "\n");
}
printf("\n");
// Clean up
for (int scanno=0; scanno<(int)scans.size(); ++scanno)
{
delete scans[scanno];
delete odometries[scanno];
}
if (random_seed)
{
delete ((RMHC_SLAM *)slam);
}
else
{
delete ((Deterministic_SLAM *)slam);
}
delete progbar;
delete mapbytes;
fclose(output);
return 0;
}

175
examples/logdemo.m
View File

@@ -1,175 +0,0 @@
%
% 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;