gfs-carmen.cpp
Go to the documentation of this file.
00001 /*****************************************************************
00002  *
00003  * This file is part of the GMAPPING project
00004  *
00005  * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
00006  * Cyrill Stachniss, and Wolfram Burgard
00007  *
00008  * This software is licensed under the "Creative Commons 
00009  * License (Attribution-NonCommercial-ShareAlike 2.0)" 
00010  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
00011  * and Wolfram Burgard.
00012  * 
00013  * Further information on this license can be found at:
00014  * http://creativecommons.org/licenses/by-nc-sa/2.0/
00015  * 
00016  * GMAPPING is distributed in the hope that it will be useful,
00017  * but WITHOUT ANY WARRANTY; without even the implied 
00018  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00019  * PURPOSE.  
00020  *
00021  *****************************************************************/
00022 
00023 
00024 #include <utils/commandline.h>
00025 #include <carmenwrapper/carmenwrapper.h>
00026 #include <gridfastslam/gridslamprocessor.h>
00027 #include <utils/orientedboundingbox.h>
00028 #include <configfile/configfile.h>
00029 
00030 #define DEBUG cout << __PRETTY_FUNCTION__
00031 
00032 /*
00033 Example file for interfacing carmen, and gfs.
00034 
00035 if you want to look for a specific topic search for one of the following keywords in the file comments
00036 
00037 KEYWORDS:
00038         CREATION
00039         INITIALIZATION
00040         SENSOR MAP
00041         BEST PARTICLE INDEX
00042         PARTICLE VECTOR
00043         PARTICLE TRAJECTORIES
00044         BEST MAP
00045         BOUNDING BOX
00046 */
00047 
00048 using namespace GMapping;
00049 using namespace std;
00050 
00051 int main(int argc, const char * const * argv){
00052         
00053         std::string outfilename="";
00054         double xmin=-100.;
00055         double ymin=-100.;
00056         double xmax=100.;
00057         double ymax=100.;
00058         double delta=0.05;
00059         
00060         //scan matching parameters
00061         double sigma=0.05;
00062         double maxrange=80.;
00063         double maxUrange=80.;
00064         double regscore=1e4;
00065         double lstep=.05;
00066         double astep=.05;
00067         int kernelSize=1;
00068         int iterations=5;
00069         double critscore=0.;
00070         double maxMove=1.;
00071         double lsigma=.075;
00072         double ogain=3;
00073         int lskip=0;
00074 
00075         //motion model parameters
00076         double srr=0.01, srt=0.01, str=0.01, stt=0.01;
00077         //particle parameters
00078         int particles=30;
00079         
00080         
00081         //gfs parameters
00082         double angularUpdate=0.5;
00083         double linearUpdate=1;
00084         double resampleThreshold=0.5;
00085         bool generateMap=true;
00086         
00087         std::string configfilename = "";
00088 
00089         CMD_PARSE_BEGIN_SILENT(1,argc);
00090                 parseStringSilent("-cfg",configfilename);
00091         CMD_PARSE_END_SILENT;
00092 
00093         if (configfilename.length()>0){
00094           ConfigFile cfg(configfilename);
00095           outfilename = (std::string) cfg.value("gfs","outfilename",outfilename);
00096           xmin = cfg.value("gfs","xmin", xmin);
00097           xmax = cfg.value("gfs","xmax",xmax);
00098           ymin = cfg.value("gfs","ymin",ymin);
00099           ymax = cfg.value("gfs","ymax",ymax);
00100           delta =  cfg.value("gfs","delta",delta);
00101           maxrange = cfg.value("gfs","maxrange",maxrange);
00102           maxUrange = cfg.value("gfs","maxUrange",maxUrange);
00103           regscore = cfg.value("gfs","regscore",regscore);
00104           critscore = cfg.value("gfs","critscore",critscore);
00105           kernelSize = cfg.value("gfs","kernelSize",kernelSize);
00106           sigma = cfg.value("gfs","sigma",sigma);
00107           iterations = cfg.value("gfs","iterations",iterations);
00108           lstep = cfg.value("gfs","lstep",lstep);
00109           astep = cfg.value("gfs","astep",astep);
00110           maxMove = cfg.value("gfs","maxMove",maxMove);
00111           srr = cfg.value("gfs","srr", srr);
00112           srt = cfg.value("gfs","srt", srt);
00113           str = cfg.value("gfs","str", str);
00114           stt = cfg.value("gfs","stt", stt);
00115           particles = cfg.value("gfs","particles",particles);
00116           angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate);
00117           linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate);
00118           lsigma = cfg.value("gfs","lsigma", lsigma);
00119           ogain = cfg.value("gfs","lobsGain", ogain);
00120           lskip = (int)cfg.value("gfs","lskip", lskip);
00121           //      randseed = cfg.value("gfs","randseed", randseed);
00122           resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
00123           generateMap = cfg.value("gfs","generateMap", generateMap);
00124         }
00125 
00126         
00127         CMD_PARSE_BEGIN(1,argc);
00128                 parseString("-cfg",configfilename);
00129                 parseString("-outfilename",outfilename);
00130                 parseDouble("-xmin",xmin);
00131                 parseDouble("-xmax",xmax);
00132                 parseDouble("-ymin",ymin);
00133                 parseDouble("-ymax",ymax);
00134                 parseDouble("-delta",delta);
00135                 parseDouble("-maxrange",maxrange);
00136                 parseDouble("-maxUrange",maxUrange);
00137                 parseDouble("-regscore",regscore);
00138                 parseDouble("-critscore",critscore);
00139                 parseInt("-kernelSize",kernelSize);
00140                 parseDouble("-sigma",sigma);
00141                 parseInt("-iterations",iterations);
00142                 parseDouble("-lstep",lstep);
00143                 parseDouble("-astep",astep);
00144                 parseDouble("-maxMove",maxMove);
00145                 parseDouble("-srr", srr);
00146                 parseDouble("-srt", srt);
00147                 parseDouble("-str", str);
00148                 parseDouble("-stt", stt);
00149                 parseInt("-particles",particles);
00150                 parseDouble("-angularUpdate", angularUpdate);
00151                 parseDouble("-linearUpdate", linearUpdate);
00152                 parseDouble("-lsigma", lsigma);
00153                 parseDouble("-lobsGain", ogain);
00154                 parseInt("-lskip", lskip);
00155                 parseDouble("-resampleThreshold", resampleThreshold);
00156                 parseFlag("-generateMap", generateMap);
00157         CMD_PARSE_END;
00158 
00159         cerr << "Parameter parsed, connecting to Carmen!";
00160 
00161         CarmenWrapper::initializeIPC(argv[0]);
00162         CarmenWrapper::start(argv[0]);
00163 
00164         while (! CarmenWrapper::sensorMapComputed()){
00165                 usleep(500000);
00166                 cerr << "." << flush;
00167         }
00168 
00169         //CREATION
00170         
00171         GridSlamProcessor* processor=new GridSlamProcessor;
00172                 
00173         //SENSOR MAP 
00174         //loads from the carmen wrapper the laser and robot settings
00175         SensorMap sensorMap=CarmenWrapper::sensorMap();
00176         cerr << "Connected " << endl;
00177         processor->setSensorMap(sensorMap);
00178 
00179         //set the command line parameters
00180         processor->setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, lsigma, ogain, lskip);
00181         processor->setMotionModelParameters(srr, srt, str, stt);
00182         processor->setUpdateDistances(linearUpdate, angularUpdate, resampleThreshold);
00183         processor->setgenerateMap(generateMap);
00184         OrientedPoint initialPose(xmin+xmax/2, ymin+ymax/2, 0);
00185         
00186         
00187         //INITIALIZATION
00188         processor->init(particles, xmin, ymin, xmax, ymax, delta, initialPose);
00189         if (outfilename.length()>0)
00190                 processor->outputStream().open(outfilename.c_str());
00191         
00192         bool running=true;
00193         
00194         GridSlamProcessor* ap, *copy=processor->clone();
00195         ap=processor; processor=copy; copy=ap;
00196 
00197         //this is the CORE LOOP;        
00198         RangeReading rr(0,0);
00199         while (running){
00200                 while (CarmenWrapper::getReading(rr)){
00201 
00202                                   
00203                         bool processed=processor->processScan(rr);
00204                         
00205                         //this returns true when the algorithm effectively processes (the traveled path since the last processing is over a given threshold)
00206                         if (processed){
00207                                 cerr << "PROCESSED" << endl;
00208                                 //for searching for the BEST PARTICLE INDEX
00209                                 //                              unsigned int best_idx=processor->getBestParticleIndex();
00210                                 
00211                                 //if you want to access to the PARTICLE VECTOR
00212                                 const GridSlamProcessor::ParticleVector& particles = processor->getParticles(); 
00213                                 //remember to use a const reference, otherwise it copys the whole particles and maps
00214                                 
00215                                 //this is for recovering the tree of PARTICLE TRAJECTORIES (obtaining the ancestor of each particle)
00216                                 cerr << "Particle reproduction story begin" << endl;
00217                                 for (unsigned int i=0; i<particles.size(); i++){
00218                                         cerr << particles[i].previousIndex << "->"  << i << " ";
00219                                 }
00220                                 cerr << "Particle reproduction story end" << endl;
00221 /*                              
00222                                 //then if you want to access the BEST MAP,
00223                                 //of course by copying it in a plain structure 
00224                                 Map<double, DoubleArray2D, false>* mymap = processor->getParticles()[best_idx].map.toDoubleMap();
00225                                 //at this point mymap is yours. Can do what you want.
00226                                                                                                 
00227                                 double best_weight=particles[best_idx].weightSum;
00228                                 cerr << "Best Particle is " << best_idx << " with weight " << best_weight << endl;
00229                                 
00230 */                              
00231                                 cerr << __PRETTY_FUNCTION__  << "CLONING... " << endl;
00232                                 GridSlamProcessor* newProcessor=processor->clone();
00233                                 cerr << "DONE" << endl;
00234                                 cerr << __PRETTY_FUNCTION__  << "DELETING... " << endl;
00235                                 delete processor;
00236                                 cerr << "DONE" << endl;
00237                                 processor=newProcessor;
00238                         } 
00239                 }
00240         }
00241         return 0;
00242 }
00243 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:12