gfs-carmen.cpp
Go to the documentation of this file.
1 /*****************************************************************
2  *
3  * This file is part of the GMAPPING project
4  *
5  * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
6  * Cyrill Stachniss, and Wolfram Burgard
7  *
8  * This software is licensed under the "Creative Commons
9  * License (Attribution-NonCommercial-ShareAlike 2.0)"
10  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
11  * and Wolfram Burgard.
12  *
13  * Further information on this license can be found at:
14  * http://creativecommons.org/licenses/by-nc-sa/2.0/
15  *
16  * GMAPPING is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied
18  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
19  * PURPOSE.
20  *
21  *****************************************************************/
22 
23 
24 #include <utils/commandline.h>
28 #include <configfile/configfile.h>
29 
30 #define DEBUG cout << __PRETTY_FUNCTION__
31 
32 /*
33 Example file for interfacing carmen, and gfs.
34 
35 if you want to look for a specific topic search for one of the following keywords in the file comments
36 
37 KEYWORDS:
38  CREATION
39  INITIALIZATION
40  SENSOR MAP
41  BEST PARTICLE INDEX
42  PARTICLE VECTOR
43  PARTICLE TRAJECTORIES
44  BEST MAP
45  BOUNDING BOX
46 */
47 
48 using namespace GMapping;
49 using namespace std;
50 
51 int main(int argc, const char * const * argv){
52 
53  std::string outfilename="";
54  double xmin=-100.;
55  double ymin=-100.;
56  double xmax=100.;
57  double ymax=100.;
58  double delta=0.05;
59 
60  //scan matching parameters
61  double sigma=0.05;
62  double maxrange=80.;
63  double maxUrange=80.;
64  double regscore=1e4;
65  double lstep=.05;
66  double astep=.05;
67  int kernelSize=1;
68  int iterations=5;
69  double critscore=0.;
70  double maxMove=1.;
71  double lsigma=.075;
72  double ogain=3;
73  int lskip=0;
74 
75  //motion model parameters
76  double srr=0.01, srt=0.01, str=0.01, stt=0.01;
77  //particle parameters
78  int particles=30;
79 
80 
81  //gfs parameters
82  double angularUpdate=0.5;
83  double linearUpdate=1;
84  double resampleThreshold=0.5;
85  bool generateMap=true;
86 
87  std::string configfilename = "";
88 
89  CMD_PARSE_BEGIN_SILENT(1,argc);
90  parseStringSilent("-cfg",configfilename);
92 
93  if (configfilename.length()>0){
94  ConfigFile cfg(configfilename);
95  outfilename = (std::string) cfg.value("gfs","outfilename",outfilename);
96  xmin = cfg.value("gfs","xmin", xmin);
97  xmax = cfg.value("gfs","xmax",xmax);
98  ymin = cfg.value("gfs","ymin",ymin);
99  ymax = cfg.value("gfs","ymax",ymax);
100  delta = cfg.value("gfs","delta",delta);
101  maxrange = cfg.value("gfs","maxrange",maxrange);
102  maxUrange = cfg.value("gfs","maxUrange",maxUrange);
103  regscore = cfg.value("gfs","regscore",regscore);
104  critscore = cfg.value("gfs","critscore",critscore);
105  kernelSize = cfg.value("gfs","kernelSize",kernelSize);
106  sigma = cfg.value("gfs","sigma",sigma);
107  iterations = cfg.value("gfs","iterations",iterations);
108  lstep = cfg.value("gfs","lstep",lstep);
109  astep = cfg.value("gfs","astep",astep);
110  maxMove = cfg.value("gfs","maxMove",maxMove);
111  srr = cfg.value("gfs","srr", srr);
112  srt = cfg.value("gfs","srt", srt);
113  str = cfg.value("gfs","str", str);
114  stt = cfg.value("gfs","stt", stt);
115  particles = cfg.value("gfs","particles",particles);
116  angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate);
117  linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate);
118  lsigma = cfg.value("gfs","lsigma", lsigma);
119  ogain = cfg.value("gfs","lobsGain", ogain);
120  lskip = (int)cfg.value("gfs","lskip", lskip);
121  // randseed = cfg.value("gfs","randseed", randseed);
122  resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
123  generateMap = cfg.value("gfs","generateMap", generateMap);
124  }
125 
126 
127  CMD_PARSE_BEGIN(1,argc);
128  parseString("-cfg",configfilename);
129  parseString("-outfilename",outfilename);
130  parseDouble("-xmin",xmin);
131  parseDouble("-xmax",xmax);
132  parseDouble("-ymin",ymin);
133  parseDouble("-ymax",ymax);
134  parseDouble("-delta",delta);
135  parseDouble("-maxrange",maxrange);
136  parseDouble("-maxUrange",maxUrange);
137  parseDouble("-regscore",regscore);
138  parseDouble("-critscore",critscore);
139  parseInt("-kernelSize",kernelSize);
140  parseDouble("-sigma",sigma);
141  parseInt("-iterations",iterations);
142  parseDouble("-lstep",lstep);
143  parseDouble("-astep",astep);
144  parseDouble("-maxMove",maxMove);
145  parseDouble("-srr", srr);
146  parseDouble("-srt", srt);
147  parseDouble("-str", str);
148  parseDouble("-stt", stt);
149  parseInt("-particles",particles);
150  parseDouble("-angularUpdate", angularUpdate);
151  parseDouble("-linearUpdate", linearUpdate);
152  parseDouble("-lsigma", lsigma);
153  parseDouble("-lobsGain", ogain);
154  parseInt("-lskip", lskip);
155  parseDouble("-resampleThreshold", resampleThreshold);
156  parseFlag("-generateMap", generateMap);
158 
159  cerr << "Parameter parsed, connecting to Carmen!";
160 
162  CarmenWrapper::start(argv[0]);
163 
165  usleep(500000);
166  cerr << "." << flush;
167  }
168 
169  //CREATION
170 
171  GridSlamProcessor* processor=new GridSlamProcessor;
172 
173  //SENSOR MAP
174  //loads from the carmen wrapper the laser and robot settings
176  cerr << "Connected " << endl;
177  processor->setSensorMap(sensorMap);
178 
179  //set the command line parameters
180  processor->setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, lsigma, ogain, lskip);
181  processor->setMotionModelParameters(srr, srt, str, stt);
182  processor->setUpdateDistances(linearUpdate, angularUpdate, resampleThreshold);
183  processor->setgenerateMap(generateMap);
184  OrientedPoint initialPose(xmin+xmax/2, ymin+ymax/2, 0);
185 
186 
187  //INITIALIZATION
188  processor->init(particles, xmin, ymin, xmax, ymax, delta, initialPose);
189  if (outfilename.length()>0)
190  processor->outputStream().open(outfilename.c_str());
191 
192  bool running=true;
193 
194  GridSlamProcessor* ap, *copy=processor->clone();
195  ap=processor; processor=copy; copy=ap;
196 
197  //this is the CORE LOOP;
198  RangeReading rr(0,0);
199  while (running){
200  while (CarmenWrapper::getReading(rr)){
201 
202 
203  bool processed=processor->processScan(rr);
204 
205  //this returns true when the algorithm effectively processes (the traveled path since the last processing is over a given threshold)
206  if (processed){
207  cerr << "PROCESSED" << endl;
208  //for searching for the BEST PARTICLE INDEX
209  // unsigned int best_idx=processor->getBestParticleIndex();
210 
211  //if you want to access to the PARTICLE VECTOR
212  const GridSlamProcessor::ParticleVector& particles = processor->getParticles();
213  //remember to use a const reference, otherwise it copys the whole particles and maps
214 
215  //this is for recovering the tree of PARTICLE TRAJECTORIES (obtaining the ancestor of each particle)
216  cerr << "Particle reproduction story begin" << endl;
217  for (unsigned int i=0; i<particles.size(); i++){
218  cerr << particles[i].previousIndex << "->" << i << " ";
219  }
220  cerr << "Particle reproduction story end" << endl;
221 /*
222  //then if you want to access the BEST MAP,
223  //of course by copying it in a plain structure
224  Map<double, DoubleArray2D, false>* mymap = processor->getParticles()[best_idx].map.toDoubleMap();
225  //at this point mymap is yours. Can do what you want.
226 
227  double best_weight=particles[best_idx].weightSum;
228  cerr << "Best Particle is " << best_idx << " with weight " << best_weight << endl;
229 
230 */
231  cerr << __PRETTY_FUNCTION__ << "CLONING... " << endl;
232  GridSlamProcessor* newProcessor=processor->clone();
233  cerr << "DONE" << endl;
234  cerr << __PRETTY_FUNCTION__ << "DELETING... " << endl;
235  delete processor;
236  cerr << "DONE" << endl;
237  processor=newProcessor;
238  }
239  }
240  }
241  return 0;
242 }
243 
const char *const *argv double delta
Definition: gfs2stream.cpp:19
const ParticleVector & getParticles() const
#define parseFlag(name, value)
Definition: commandline.h:28
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0)
bool processScan(const RangeReading &reading, int adaptParticles=0)
std::vector< Particle > ParticleVector
CMD_PARSE_END
Definition: gfs2stream.cpp:32
#define parseDouble(name, value)
Definition: commandline.h:44
#define CMD_PARSE_BEGIN_SILENT(i, count)
Definition: commandline.h:73
static void initializeIPC(const char *name)
#define parseInt(name, value)
Definition: commandline.h:52
double maxrange
Definition: gfs2stream.cpp:22
void setUpdateDistances(double linear, double angular, double resampleThreshold)
void setSensorMap(const SensorMap &smap)
GridSlamProcessor * clone() const
#define parseString(name, value)
Definition: commandline.h:35
static bool sensorMapComputed()
#define CMD_PARSE_END_SILENT
Definition: commandline.h:79
static bool start(const char *name)
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:19
#define parseStringSilent(name, value)
Definition: commandline.h:91
static const SensorMap & sensorMap()
void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose=OrientedPoint(0, 0, 0))
static bool getReading(RangeReading &reading)
int main(int argc, conat char **argv)
Definition: lumiles.h:45
void setMotionModelParameters(double srr, double srt, double str, double stt)
CMD_PARSE_BEGIN(1, argc-2)


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Mon Jun 10 2019 14:04:22