X-lanni
644 lines
18 KiB
C++
644 lines
18 KiB
C++
/*****************************************************************
|
|
*
|
|
* 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 "gsp_thread.h"
|
|
#include <utils/commandline.h>
|
|
#include <utils/stat.h>
|
|
#include <configfile/configfile.h>
|
|
|
|
#ifdef CARMEN_SUPPORT
|
|
#include <carmenwrapper/carmenwrapper.h>
|
|
#endif
|
|
|
|
#define DEBUG cout << __PRETTY_FUNCTION__
|
|
|
|
using namespace std;
|
|
|
|
int GridSlamProcessorThread::init(int argc, const char * const * argv){
|
|
m_argc=argc;
|
|
m_argv=argv;
|
|
std::string configfilename;
|
|
std::string ebuf="not_set";
|
|
|
|
CMD_PARSE_BEGIN_SILENT(1,argc);
|
|
parseStringSilent("-cfg",configfilename);
|
|
CMD_PARSE_END_SILENT;
|
|
|
|
if (configfilename.length()>0){
|
|
ConfigFile cfg(configfilename);
|
|
|
|
filename = (std::string) cfg.value("gfs","filename",filename);
|
|
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);
|
|
mapUpdateTime = cfg.value("gfs","mapUpdate", mapUpdateTime);
|
|
randseed = cfg.value("gfs","randseed", randseed);
|
|
autosize = cfg.value("gfs","autosize", autosize);
|
|
readFromStdin = cfg.value("gfs","stdin", readFromStdin);
|
|
resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
|
|
skipMatching = cfg.value("gfs","skipMatching", skipMatching);
|
|
onLine = cfg.value("gfs","onLine", onLine);
|
|
generateMap = cfg.value("gfs","generateMap", generateMap);
|
|
m_minimumScore = cfg.value("gfs","minimumScore", m_minimumScore);
|
|
llsamplerange = cfg.value("gfs","llsamplerange", llsamplerange);
|
|
lasamplerange = cfg.value("gfs","lasamplerange",lasamplerange );
|
|
llsamplestep = cfg.value("gfs","llsamplestep", llsamplestep);
|
|
lasamplestep = cfg.value("gfs","lasamplestep", lasamplestep);
|
|
linearOdometryReliability = cfg.value("gfs","linearOdometryReliability",linearOdometryReliability);
|
|
angularOdometryReliability = cfg.value("gfs","angularOdometryReliability",angularOdometryReliability);
|
|
ebuf = (std::string) cfg.value("gfs","estrategy", ebuf);
|
|
considerOdometryCovariance = cfg.value("gfs","considerOdometryCovariance",considerOdometryCovariance);
|
|
|
|
}
|
|
|
|
|
|
CMD_PARSE_BEGIN(1,argc);
|
|
parseString("-cfg",configfilename); /* to avoid the warning*/
|
|
parseString("-filename",filename);
|
|
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);
|
|
parseInt("-mapUpdate", mapUpdateTime);
|
|
parseInt("-randseed", randseed);
|
|
parseFlag("-autosize", autosize);
|
|
parseFlag("-stdin", readFromStdin);
|
|
parseDouble("-resampleThreshold", resampleThreshold);
|
|
parseFlag("-skipMatching", skipMatching);
|
|
parseFlag("-onLine", onLine);
|
|
parseFlag("-generateMap", generateMap);
|
|
parseDouble("-minimumScore", m_minimumScore);
|
|
parseDouble("-llsamplerange", llsamplerange);
|
|
parseDouble("-lasamplerange", lasamplerange);
|
|
parseDouble("-llsamplestep", llsamplestep);
|
|
parseDouble("-lasamplestep", lasamplestep);
|
|
parseDouble("-linearOdometryReliability",linearOdometryReliability);
|
|
parseDouble("-angularOdometryReliability",angularOdometryReliability);
|
|
parseString("-estrategy", ebuf);
|
|
|
|
parseFlag("-considerOdometryCovariance",considerOdometryCovariance);
|
|
CMD_PARSE_END;
|
|
|
|
if (filename.length() <=0){
|
|
cout << "no filename specified" << endl;
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int GridSlamProcessorThread::loadFiles(const char * fn){
|
|
if (onLine){
|
|
cout << " onLineProcessing" << endl;
|
|
return 0;
|
|
}
|
|
ifstream is;
|
|
if (fn)
|
|
is.open(fn);
|
|
else
|
|
is.open(filename.c_str());
|
|
if (! is){
|
|
cout << "no file found" << endl;
|
|
return -1;
|
|
}
|
|
|
|
CarmenConfiguration conf;
|
|
conf.load(is);
|
|
is.close();
|
|
|
|
sensorMap=conf.computeSensorMap();
|
|
|
|
if (input)
|
|
delete input;
|
|
|
|
if (! readFromStdin){
|
|
plainStream.open(filename.c_str());
|
|
input=new InputSensorStream(sensorMap, plainStream);
|
|
cout << "Plain Stream opened="<< (bool) plainStream << endl;
|
|
} else {
|
|
input=new InputSensorStream(sensorMap, cin);
|
|
cout << "Plain Stream opened on stdin" << endl;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
GridSlamProcessorThread::GridSlamProcessorThread(): GridSlamProcessor(cerr){
|
|
//This are the processor parameters
|
|
filename="";
|
|
outfilename="";
|
|
xmin=-100.;
|
|
ymin=-100.;
|
|
xmax=100.;
|
|
ymax=100.;
|
|
delta=0.05;
|
|
|
|
//scan matching parameters
|
|
sigma=0.05;
|
|
maxrange=80.;
|
|
maxUrange=80.;
|
|
regscore=1e4;
|
|
lstep=.05;
|
|
astep=.05;
|
|
kernelSize=1;
|
|
iterations=5;
|
|
critscore=0.;
|
|
maxMove=1.;
|
|
lsigma=.075;
|
|
ogain=3;
|
|
lskip=0;
|
|
autosize=false;
|
|
skipMatching=false;
|
|
|
|
//motion model parameters
|
|
srr=0.1, srt=0.1, str=0.1, stt=0.1;
|
|
//particle parameters
|
|
particles=30;
|
|
randseed=0;
|
|
|
|
//gfs parameters
|
|
angularUpdate=0.5;
|
|
linearUpdate=1;
|
|
resampleThreshold=0.5;
|
|
|
|
input=0;
|
|
|
|
pthread_mutex_init(&hp_mutex,0);
|
|
pthread_mutex_init(&ind_mutex,0);
|
|
pthread_mutex_init(&hist_mutex,0);
|
|
running=false;
|
|
eventBufferLength=0;
|
|
mapUpdateTime=5;
|
|
mapTimer=0;
|
|
readFromStdin=false;
|
|
onLine=false;
|
|
generateMap=false;
|
|
|
|
// This are the dafault settings for a grid map of 5 cm
|
|
llsamplerange=0.01;
|
|
llsamplestep=0.01;
|
|
lasamplerange=0.005;
|
|
lasamplestep=0.005;
|
|
linearOdometryReliability=0.;
|
|
angularOdometryReliability=0.;
|
|
|
|
considerOdometryCovariance=false;
|
|
/*
|
|
// This are the dafault settings for a grid map of 10 cm
|
|
m_llsamplerange=0.1;
|
|
m_llsamplestep=0.1;
|
|
m_lasamplerange=0.02;
|
|
m_lasamplestep=0.01;
|
|
*/
|
|
// This are the dafault settings for a grid map of 20/25 cm
|
|
/*
|
|
m_llsamplerange=0.2;
|
|
m_llsamplestep=0.1;
|
|
m_lasamplerange=0.02;
|
|
m_lasamplestep=0.01;
|
|
m_generateMap=false;
|
|
*/
|
|
|
|
|
|
}
|
|
|
|
GridSlamProcessorThread::~GridSlamProcessorThread(){
|
|
pthread_mutex_destroy(&hp_mutex);
|
|
pthread_mutex_destroy(&ind_mutex);
|
|
pthread_mutex_destroy(&hist_mutex);
|
|
|
|
for (deque<Event*>::const_iterator it=eventBuffer.begin(); it!=eventBuffer.end(); it++)
|
|
delete *it;
|
|
}
|
|
|
|
|
|
void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt){
|
|
if (! gpt->input && ! gpt->onLine)
|
|
return 0;
|
|
|
|
|
|
//if started online retrieve the settings from the connection
|
|
#ifdef CARMEN_SUPPORT
|
|
if (gpt->onLine){
|
|
cout << "starting the process:" << endl;
|
|
CarmenWrapper::initializeIPC(gpt->m_argv[0]);
|
|
CarmenWrapper::start(gpt->m_argv[0]);
|
|
cout << "Waiting for retrieving the sensor map:" << endl;
|
|
while (! CarmenWrapper::sensorMapComputed()){
|
|
usleep(500000);
|
|
cout << "." << flush;
|
|
}
|
|
gpt->sensorMap=CarmenWrapper::sensorMap();
|
|
cout << "Connected " << endl;
|
|
}
|
|
#else
|
|
if (gpt->onLine){
|
|
cout << "FATAL ERROR: cannot run online without the carmen support" << endl;
|
|
DoneEvent *done=new DoneEvent;
|
|
gpt->addEvent(done);
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
gpt->setSensorMap(gpt->sensorMap);
|
|
gpt->setMatchingParameters(gpt->maxUrange, gpt->maxrange, gpt->sigma, gpt->kernelSize, gpt->lstep, gpt->astep, gpt->iterations, gpt->lsigma, gpt->ogain, gpt->lskip);
|
|
|
|
double xmin=gpt->xmin,
|
|
ymin=gpt->ymin,
|
|
xmax=gpt->xmax,
|
|
ymax=gpt->ymax;
|
|
|
|
OrientedPoint initialPose(0,0,0);
|
|
|
|
if (gpt->autosize){
|
|
if (gpt->readFromStdin || gpt->onLine)
|
|
cout << "Error, cant autosize form stdin" << endl;
|
|
SensorLog * log=new SensorLog(gpt->sensorMap);
|
|
ifstream is(gpt->filename.c_str());
|
|
log->load(is);
|
|
is.close();
|
|
initialPose=gpt->boundingBox(log, xmin, ymin, xmax, ymax);
|
|
delete log;
|
|
}
|
|
|
|
if( gpt->infoStream()){
|
|
gpt->infoStream() << " initialPose=" << initialPose.x << " " << initialPose.y << " " << initialPose.theta
|
|
<< cout << " xmin=" << xmin <<" ymin=" << ymin <<" xmax=" << xmax <<" ymax=" << ymax << endl;
|
|
}
|
|
gpt->setMotionModelParameters(gpt->srr, gpt->srt, gpt->str, gpt->stt);
|
|
gpt->setUpdateDistances(gpt->linearUpdate, gpt->angularUpdate, gpt->resampleThreshold);
|
|
gpt->setgenerateMap(gpt->generateMap);
|
|
gpt->GridSlamProcessor::init(gpt->particles, xmin, ymin, xmax, ymax, gpt->delta, initialPose);
|
|
gpt->setllsamplerange(gpt->llsamplerange);
|
|
gpt->setllsamplestep(gpt->llsamplestep);
|
|
gpt->setlasamplerange(gpt->llsamplerange);
|
|
gpt->setlasamplestep(gpt->llsamplestep);
|
|
|
|
#define printParam(n)\
|
|
gpt->outputStream() \
|
|
<< "PARAM " << \
|
|
#n \
|
|
<< " " << gpt->n << endl
|
|
|
|
if (gpt->outfilename.length()>0 ){
|
|
gpt->outputStream().open(gpt->outfilename.c_str());
|
|
printParam(filename);
|
|
printParam(outfilename);
|
|
printParam(xmin);
|
|
printParam(ymin);
|
|
printParam(xmax);
|
|
printParam(ymax);
|
|
printParam(delta);
|
|
|
|
//scan matching parameters
|
|
printParam(sigma);
|
|
printParam(maxrange);
|
|
printParam(maxUrange);
|
|
printParam(regscore);
|
|
printParam(lstep);
|
|
printParam(astep);
|
|
printParam(kernelSize);
|
|
printParam(iterations);
|
|
printParam(critscore);
|
|
printParam(maxMove);
|
|
printParam(lsigma);
|
|
printParam(ogain);
|
|
printParam(lskip);
|
|
printParam(autosize);
|
|
printParam(skipMatching);
|
|
|
|
//motion model parameters
|
|
printParam(srr);
|
|
printParam(srt);
|
|
printParam(str);
|
|
printParam(stt);
|
|
//particle parameters
|
|
printParam(particles);
|
|
printParam(randseed);
|
|
|
|
//gfs parameters
|
|
printParam(angularUpdate);
|
|
printParam(linearUpdate);
|
|
printParam(resampleThreshold);
|
|
|
|
printParam(llsamplerange);
|
|
printParam(lasamplerange);
|
|
printParam(llsamplestep);
|
|
printParam(lasamplestep);
|
|
}
|
|
#undef printParam
|
|
|
|
if (gpt->randseed!=0)
|
|
sampleGaussian(1,gpt->randseed);
|
|
if (!gpt->infoStream()){
|
|
cerr << "cant open info stream for writing by unuseful debug messages" << endl;
|
|
} else {
|
|
gpt->infoStream() << "setting randseed" << gpt->randseed<< endl;
|
|
}
|
|
|
|
|
|
#ifdef CARMEN_SUPPORT
|
|
list<RangeReading*> rrlist;
|
|
if (gpt->onLine){
|
|
RangeReading rr(0,0);
|
|
while (1){
|
|
while (CarmenWrapper::getReading(rr)){
|
|
RangeReading* nr=new RangeReading(rr);
|
|
rrlist.push_back(nr);
|
|
gpt->processScan(*nr);
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
ofstream rawpath("rawpath.dat");
|
|
if (!gpt->onLine){
|
|
while(*(gpt->input) && gpt->running){
|
|
const SensorReading* r;
|
|
(*(gpt->input)) >> r;
|
|
if (! r)
|
|
continue;
|
|
const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
|
|
if (rr && gpt->running){
|
|
const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
|
|
assert (rs && rs->beams().size()==rr->size());
|
|
|
|
bool processed=gpt->processScan(*rr);
|
|
rawpath << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << endl;
|
|
if (0 && processed){
|
|
cerr << "Retrieving state .. ";
|
|
TNodeVector trajetories=gpt->getTrajectories();
|
|
cerr << "Done" << endl;
|
|
cerr << "Deleting Tree state .. ";
|
|
for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++)
|
|
delete *it;
|
|
cerr << "Done" << endl;
|
|
}
|
|
// if (0 && processed){
|
|
// cerr << "generating copy" << endl;;
|
|
// GridSlamProcessor* m_gsp=gpt->clone();
|
|
// Map<double, DoubleArray2D, false>* pmap=m_gsp->getParticles()[0].map.toDoubleMap() ;
|
|
// cerr << "deleting" << endl;
|
|
// delete m_gsp;
|
|
// delete pmap;
|
|
// }
|
|
}
|
|
const OdometryReading* o=dynamic_cast<const OdometryReading*>(r);
|
|
if (o && gpt->running){
|
|
gpt->processTruePos(*o);
|
|
TruePosEvent* truepos=new TruePosEvent;
|
|
truepos->pose=o->getPose();
|
|
}
|
|
}
|
|
}
|
|
rawpath.close();
|
|
|
|
TNodeVector trajetories=gpt->getTrajectories();
|
|
cerr << "WRITING WEIGHTS" << endl;
|
|
int pnumber=0;
|
|
for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++){
|
|
char buf[10];
|
|
sprintf(buf, "w-%03d.dat",pnumber);
|
|
ofstream weightsStream(buf);
|
|
GridSlamProcessor::TNode* n=*it;
|
|
double oldWeight=0, oldgWeight=0;
|
|
while (n!=0){
|
|
double w=n->weight-oldWeight;
|
|
double gw=n->gweight-oldgWeight;
|
|
oldWeight=n->weight;
|
|
oldgWeight=n->gweight;
|
|
weightsStream << w << " " << gw << endl;
|
|
n=n->parent;
|
|
}
|
|
weightsStream.close();
|
|
pnumber++;
|
|
cerr << buf << endl;
|
|
}
|
|
|
|
DoneEvent *done=new DoneEvent;
|
|
gpt->addEvent(done);
|
|
gpt->infoStream() << "Hallo, I am the gsp thread. I have finished. Do you think it is the case of checking for unlocked mutexes." << endl;
|
|
return 0;
|
|
}
|
|
|
|
std::vector<OrientedPoint> GridSlamProcessorThread::getHypotheses(){
|
|
pthread_mutex_lock(&hp_mutex);
|
|
std::vector<OrientedPoint> retval(hypotheses);
|
|
pthread_mutex_unlock(&hp_mutex);
|
|
return retval;
|
|
}
|
|
|
|
std::vector<unsigned int> GridSlamProcessorThread::getIndexes(){
|
|
pthread_mutex_lock(&ind_mutex);
|
|
std::vector<unsigned int> retval(indexes);
|
|
pthread_mutex_unlock(&ind_mutex);
|
|
return retval;
|
|
}
|
|
|
|
void GridSlamProcessorThread::start(){
|
|
if (running)
|
|
return;
|
|
running=true;
|
|
pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this);
|
|
}
|
|
|
|
void GridSlamProcessorThread::stop(){
|
|
if (! running){
|
|
cout << "PORCO CAZZO" << endl;
|
|
return;
|
|
}
|
|
running=false;
|
|
void * retval;
|
|
pthread_join(gfs_thread, &retval);
|
|
}
|
|
|
|
void GridSlamProcessorThread::onOdometryUpdate(){
|
|
pthread_mutex_lock(&hp_mutex);
|
|
hypotheses.clear();
|
|
weightSums.clear();
|
|
for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
|
|
hypotheses.push_back(part->pose);
|
|
weightSums.push_back(part->weightSum);
|
|
}
|
|
|
|
ParticleMoveEvent* event=new ParticleMoveEvent;
|
|
event->scanmatched=false;
|
|
event->hypotheses=hypotheses;
|
|
event->weightSums=weightSums;
|
|
event->neff=m_neff;
|
|
pthread_mutex_unlock(&hp_mutex);
|
|
|
|
addEvent(event);
|
|
|
|
syncOdometryUpdate();
|
|
}
|
|
|
|
void GridSlamProcessorThread::onResampleUpdate(){
|
|
pthread_mutex_lock(&ind_mutex);
|
|
pthread_mutex_lock(&hp_mutex);
|
|
|
|
indexes=GridSlamProcessor::getIndexes();
|
|
|
|
assert (indexes.size()==getParticles().size());
|
|
ResampleEvent* event=new ResampleEvent;
|
|
event->indexes=indexes;
|
|
|
|
pthread_mutex_unlock(&hp_mutex);
|
|
pthread_mutex_unlock(&ind_mutex);
|
|
|
|
addEvent(event);
|
|
|
|
syncResampleUpdate();
|
|
}
|
|
|
|
void GridSlamProcessorThread::onScanmatchUpdate(){
|
|
pthread_mutex_lock(&hp_mutex);
|
|
hypotheses.clear();
|
|
weightSums.clear();
|
|
unsigned int bestIdx=0;
|
|
double bestWeight=-1e1000;
|
|
unsigned int idx=0;
|
|
for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
|
|
hypotheses.push_back(part->pose);
|
|
weightSums.push_back(part->weightSum);
|
|
if(part->weightSum>bestWeight){
|
|
bestIdx=idx;
|
|
bestWeight=part->weightSum;
|
|
}
|
|
idx++;
|
|
}
|
|
|
|
ParticleMoveEvent* event=new ParticleMoveEvent;
|
|
event->scanmatched=true;
|
|
event->hypotheses=hypotheses;
|
|
event->weightSums=weightSums;
|
|
event->neff=m_neff;
|
|
addEvent(event);
|
|
|
|
if (! mapTimer){
|
|
MapEvent* event=new MapEvent;
|
|
event->index=bestIdx;
|
|
event->pmap=new ScanMatcherMap(getParticles()[bestIdx].map);
|
|
event->pose=getParticles()[bestIdx].pose;
|
|
addEvent(event);
|
|
}
|
|
|
|
mapTimer++;
|
|
mapTimer=mapTimer%mapUpdateTime;
|
|
|
|
pthread_mutex_unlock(&hp_mutex);
|
|
|
|
syncOdometryUpdate();
|
|
}
|
|
|
|
void GridSlamProcessorThread::syncOdometryUpdate(){
|
|
}
|
|
|
|
void GridSlamProcessorThread::syncResampleUpdate(){
|
|
}
|
|
|
|
void GridSlamProcessorThread::syncScanmatchUpdate(){
|
|
}
|
|
|
|
void GridSlamProcessorThread::addEvent(GridSlamProcessorThread::Event * e){
|
|
pthread_mutex_lock(&hist_mutex);
|
|
while (eventBuffer.size()>eventBufferLength){
|
|
Event* event=eventBuffer.front();
|
|
delete event;
|
|
eventBuffer.pop_front();
|
|
}
|
|
eventBuffer.push_back(e);
|
|
pthread_mutex_unlock(&hist_mutex);
|
|
}
|
|
|
|
GridSlamProcessorThread::EventDeque GridSlamProcessorThread::getEvents(){
|
|
pthread_mutex_lock(&hist_mutex);
|
|
EventDeque copy(eventBuffer);
|
|
eventBuffer.clear();
|
|
pthread_mutex_unlock(&hist_mutex);
|
|
return copy;
|
|
}
|
|
|
|
GridSlamProcessorThread::Event::~Event(){}
|
|
|
|
GridSlamProcessorThread::MapEvent::~MapEvent(){
|
|
if (pmap)
|
|
delete pmap;
|
|
}
|
|
|
|
void GridSlamProcessorThread::setEventBufferSize(unsigned int length){
|
|
eventBufferLength=length;
|
|
}
|
|
|
|
OrientedPoint GridSlamProcessorThread::boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const{
|
|
OrientedPoint initialPose(0,0,0);
|
|
initialPose=log->boundingBox(xmin, ymin, xmax, ymax);
|
|
xmin-=3*maxrange;
|
|
ymin-=3*maxrange;
|
|
xmax+=3*maxrange;
|
|
ymax+=3*maxrange;
|
|
return initialPose;
|
|
}
|