add slam_gmapping
This commit is contained in:
X-lanni
@@ -0,0 +1,21 @@
|
||||
OBJS=
|
||||
APPS= gfs-carmen
|
||||
|
||||
LIBS+= -lcarmenwrapper -lgridfastslam -lconfigfile
|
||||
CPPFLAGS+= -I ../sensor -I$(CARMEN_HOME)/include
|
||||
|
||||
-include ../global.mk
|
||||
ifeq ($(CARMENSUPPORT), 0)
|
||||
APPS=
|
||||
.PHONY: clean all
|
||||
|
||||
all:
|
||||
|
||||
clean:
|
||||
|
||||
else
|
||||
-include ../build_tools/Makefile.app
|
||||
endif
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,243 @@
|
||||
/*****************************************************************
|
||||
*
|
||||
* This file is part of the GMAPPING project
|
||||
*
|
||||
* GMAPPING Copyright (c) 2004 Giorgio Grisetti,
|
||||
* Cyrill Stachniss, and Wolfram Burgard
|
||||
*
|
||||
* This software is licensed under the "Creative Commons
|
||||
* License (Attribution-NonCommercial-ShareAlike 2.0)"
|
||||
* and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
|
||||
* and Wolfram Burgard.
|
||||
*
|
||||
* Further information on this license can be found at:
|
||||
* http://creativecommons.org/licenses/by-nc-sa/2.0/
|
||||
*
|
||||
* GMAPPING 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.
|
||||
*
|
||||
*****************************************************************/
|
||||
|
||||
|
||||
#include <utils/commandline.h>
|
||||
#include <carmenwrapper/carmenwrapper.h>
|
||||
#include <gridfastslam/gridslamprocessor.h>
|
||||
#include <utils/orientedboundingbox.h>
|
||||
#include <configfile/configfile.h>
|
||||
|
||||
#define DEBUG cout << __PRETTY_FUNCTION__
|
||||
|
||||
/*
|
||||
Example file for interfacing carmen, and gfs.
|
||||
|
||||
if you want to look for a specific topic search for one of the following keywords in the file comments
|
||||
|
||||
KEYWORDS:
|
||||
CREATION
|
||||
INITIALIZATION
|
||||
SENSOR MAP
|
||||
BEST PARTICLE INDEX
|
||||
PARTICLE VECTOR
|
||||
PARTICLE TRAJECTORIES
|
||||
BEST MAP
|
||||
BOUNDING BOX
|
||||
*/
|
||||
|
||||
using namespace GMapping;
|
||||
using namespace std;
|
||||
|
||||
int main(int argc, const char * const * argv){
|
||||
|
||||
std::string outfilename="";
|
||||
double xmin=-100.;
|
||||
double ymin=-100.;
|
||||
double xmax=100.;
|
||||
double ymax=100.;
|
||||
double delta=0.05;
|
||||
|
||||
//scan matching parameters
|
||||
double sigma=0.05;
|
||||
double maxrange=80.;
|
||||
double maxUrange=80.;
|
||||
double regscore=1e4;
|
||||
double lstep=.05;
|
||||
double astep=.05;
|
||||
int kernelSize=1;
|
||||
int iterations=5;
|
||||
double critscore=0.;
|
||||
double maxMove=1.;
|
||||
double lsigma=.075;
|
||||
double ogain=3;
|
||||
int lskip=0;
|
||||
|
||||
//motion model parameters
|
||||
double srr=0.01, srt=0.01, str=0.01, stt=0.01;
|
||||
//particle parameters
|
||||
int particles=30;
|
||||
|
||||
|
||||
//gfs parameters
|
||||
double angularUpdate=0.5;
|
||||
double linearUpdate=1;
|
||||
double resampleThreshold=0.5;
|
||||
bool generateMap=true;
|
||||
|
||||
std::string configfilename = "";
|
||||
|
||||
CMD_PARSE_BEGIN_SILENT(1,argc);
|
||||
parseStringSilent("-cfg",configfilename);
|
||||
CMD_PARSE_END_SILENT;
|
||||
|
||||
if (configfilename.length()>0){
|
||||
ConfigFile cfg(configfilename);
|
||||
outfilename = (std::string) cfg.value("gfs","outfilename",outfilename);
|
||||
xmin = cfg.value("gfs","xmin", xmin);
|
||||
xmax = cfg.value("gfs","xmax",xmax);
|
||||
ymin = cfg.value("gfs","ymin",ymin);
|
||||
ymax = cfg.value("gfs","ymax",ymax);
|
||||
delta = cfg.value("gfs","delta",delta);
|
||||
maxrange = cfg.value("gfs","maxrange",maxrange);
|
||||
maxUrange = cfg.value("gfs","maxUrange",maxUrange);
|
||||
regscore = cfg.value("gfs","regscore",regscore);
|
||||
critscore = cfg.value("gfs","critscore",critscore);
|
||||
kernelSize = cfg.value("gfs","kernelSize",kernelSize);
|
||||
sigma = cfg.value("gfs","sigma",sigma);
|
||||
iterations = cfg.value("gfs","iterations",iterations);
|
||||
lstep = cfg.value("gfs","lstep",lstep);
|
||||
astep = cfg.value("gfs","astep",astep);
|
||||
maxMove = cfg.value("gfs","maxMove",maxMove);
|
||||
srr = cfg.value("gfs","srr", srr);
|
||||
srt = cfg.value("gfs","srt", srt);
|
||||
str = cfg.value("gfs","str", str);
|
||||
stt = cfg.value("gfs","stt", stt);
|
||||
particles = cfg.value("gfs","particles",particles);
|
||||
angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate);
|
||||
linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate);
|
||||
lsigma = cfg.value("gfs","lsigma", lsigma);
|
||||
ogain = cfg.value("gfs","lobsGain", ogain);
|
||||
lskip = (int)cfg.value("gfs","lskip", lskip);
|
||||
// randseed = cfg.value("gfs","randseed", randseed);
|
||||
resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
|
||||
generateMap = cfg.value("gfs","generateMap", generateMap);
|
||||
}
|
||||
|
||||
|
||||
CMD_PARSE_BEGIN(1,argc);
|
||||
parseString("-cfg",configfilename);
|
||||
parseString("-outfilename",outfilename);
|
||||
parseDouble("-xmin",xmin);
|
||||
parseDouble("-xmax",xmax);
|
||||
parseDouble("-ymin",ymin);
|
||||
parseDouble("-ymax",ymax);
|
||||
parseDouble("-delta",delta);
|
||||
parseDouble("-maxrange",maxrange);
|
||||
parseDouble("-maxUrange",maxUrange);
|
||||
parseDouble("-regscore",regscore);
|
||||
parseDouble("-critscore",critscore);
|
||||
parseInt("-kernelSize",kernelSize);
|
||||
parseDouble("-sigma",sigma);
|
||||
parseInt("-iterations",iterations);
|
||||
parseDouble("-lstep",lstep);
|
||||
parseDouble("-astep",astep);
|
||||
parseDouble("-maxMove",maxMove);
|
||||
parseDouble("-srr", srr);
|
||||
parseDouble("-srt", srt);
|
||||
parseDouble("-str", str);
|
||||
parseDouble("-stt", stt);
|
||||
parseInt("-particles",particles);
|
||||
parseDouble("-angularUpdate", angularUpdate);
|
||||
parseDouble("-linearUpdate", linearUpdate);
|
||||
parseDouble("-lsigma", lsigma);
|
||||
parseDouble("-lobsGain", ogain);
|
||||
parseInt("-lskip", lskip);
|
||||
parseDouble("-resampleThreshold", resampleThreshold);
|
||||
parseFlag("-generateMap", generateMap);
|
||||
CMD_PARSE_END;
|
||||
|
||||
cerr << "Parameter parsed, connecting to Carmen!";
|
||||
|
||||
CarmenWrapper::initializeIPC(argv[0]);
|
||||
CarmenWrapper::start(argv[0]);
|
||||
|
||||
while (! CarmenWrapper::sensorMapComputed()){
|
||||
usleep(500000);
|
||||
cerr << "." << flush;
|
||||
}
|
||||
|
||||
//CREATION
|
||||
|
||||
GridSlamProcessor* processor=new GridSlamProcessor;
|
||||
|
||||
//SENSOR MAP
|
||||
//loads from the carmen wrapper the laser and robot settings
|
||||
SensorMap sensorMap=CarmenWrapper::sensorMap();
|
||||
cerr << "Connected " << endl;
|
||||
processor->setSensorMap(sensorMap);
|
||||
|
||||
//set the command line parameters
|
||||
processor->setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, lsigma, ogain, lskip);
|
||||
processor->setMotionModelParameters(srr, srt, str, stt);
|
||||
processor->setUpdateDistances(linearUpdate, angularUpdate, resampleThreshold);
|
||||
processor->setgenerateMap(generateMap);
|
||||
OrientedPoint initialPose(xmin+xmax/2, ymin+ymax/2, 0);
|
||||
|
||||
|
||||
//INITIALIZATION
|
||||
processor->init(particles, xmin, ymin, xmax, ymax, delta, initialPose);
|
||||
if (outfilename.length()>0)
|
||||
processor->outputStream().open(outfilename.c_str());
|
||||
|
||||
bool running=true;
|
||||
|
||||
GridSlamProcessor* ap, *copy=processor->clone();
|
||||
ap=processor; processor=copy; copy=ap;
|
||||
|
||||
//this is the CORE LOOP;
|
||||
RangeReading rr(0,0);
|
||||
while (running){
|
||||
while (CarmenWrapper::getReading(rr)){
|
||||
|
||||
|
||||
bool processed=processor->processScan(rr);
|
||||
|
||||
//this returns true when the algorithm effectively processes (the traveled path since the last processing is over a given threshold)
|
||||
if (processed){
|
||||
cerr << "PROCESSED" << endl;
|
||||
//for searching for the BEST PARTICLE INDEX
|
||||
// unsigned int best_idx=processor->getBestParticleIndex();
|
||||
|
||||
//if you want to access to the PARTICLE VECTOR
|
||||
const GridSlamProcessor::ParticleVector& particles = processor->getParticles();
|
||||
//remember to use a const reference, otherwise it copys the whole particles and maps
|
||||
|
||||
//this is for recovering the tree of PARTICLE TRAJECTORIES (obtaining the ancestor of each particle)
|
||||
cerr << "Particle reproduction story begin" << endl;
|
||||
for (unsigned int i=0; i<particles.size(); i++){
|
||||
cerr << particles[i].previousIndex << "->" << i << " ";
|
||||
}
|
||||
cerr << "Particle reproduction story end" << endl;
|
||||
/*
|
||||
//then if you want to access the BEST MAP,
|
||||
//of course by copying it in a plain structure
|
||||
Map<double, DoubleArray2D, false>* mymap = processor->getParticles()[best_idx].map.toDoubleMap();
|
||||
//at this point mymap is yours. Can do what you want.
|
||||
|
||||
double best_weight=particles[best_idx].weightSum;
|
||||
cerr << "Best Particle is " << best_idx << " with weight " << best_weight << endl;
|
||||
|
||||
*/
|
||||
cerr << __PRETTY_FUNCTION__ << "CLONING... " << endl;
|
||||
GridSlamProcessor* newProcessor=processor->clone();
|
||||
cerr << "DONE" << endl;
|
||||
cerr << __PRETTY_FUNCTION__ << "DELETING... " << endl;
|
||||
delete processor;
|
||||
cerr << "DONE" << endl;
|
||||
processor=newProcessor;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user