441 lines
11 KiB
C++
441 lines
11 KiB
C++
/*
|
|
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 "Velocities.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
|
|
{
|
|
}
|
|
|
|
Velocities computeVelocities(long * odometry, Velocities & velocities)
|
|
{
|
|
return WheeledRobot::computeVelocities(
|
|
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)
|
|
{
|
|
Velocities velocities = robot.computeVelocities(odometries[scanno], velocities);
|
|
slam->update(lidar, velocities);
|
|
}
|
|
else
|
|
{
|
|
((RMHC_SLAM *)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;
|
|
}
|