gsp_thread.h
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 #ifndef GSP_THREAD_H
25 #define GSP_THREAD_H
26 
27 #include <unistd.h>
28 #include <pthread.h>
29 #include <deque>
30 #include <fstream>
31 #include <iostream>
33 #include <log/sensorstream.h>
35 
36 using namespace std;
37 using namespace GMapping;
38 
39 
40 #define MAX_STRING_LENGTH 1024
41 
42 
44  struct Event{
45  virtual ~Event();
46  };
47 
48  struct ParticleMoveEvent: public Event{
50  double neff;
51  std::vector<OrientedPoint> hypotheses;
52  std::vector<double> weightSums;
53  };
54 
55  struct TruePosEvent : public Event{
57  };
58 
59  struct ResampleEvent: public Event{
60  std::vector<unsigned int> indexes;
61  };
62 
63  struct MapEvent: public Event{
65  unsigned int index;
67  virtual ~MapEvent();
68  };
69 
70  struct DoneEvent: public Event{
71  };
72 
73  typedef deque<Event*> EventDeque;
74 
77  int init(int argc, const char * const * argv);
78  int loadFiles(const char * fn=0);
79  static void * fastslamthread(GridSlamProcessorThread* gpt);
80  std::vector<OrientedPoint> getHypotheses();
81  std::vector<unsigned int> getIndexes();
82 
83  EventDeque getEvents();
84 
85  void start();
86  void stop();
87 
88  virtual void onOdometryUpdate();
89  virtual void onResampleUpdate();
90  virtual void onScanmatchUpdate();
91 
92  virtual void syncOdometryUpdate();
93  virtual void syncResampleUpdate();
94  virtual void syncScanmatchUpdate();
95 
96  void setEventBufferSize(unsigned int length);
97  inline void setMapUpdateTime(unsigned int ut) {mapUpdateTime=ut;}
98  inline bool isRunning() const {return running;}
99  OrientedPoint boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const;
100  private:
101 
102  void addEvent(Event *);
103  EventDeque eventBuffer;
104 
105  unsigned int eventBufferLength;
106  unsigned int mapUpdateTime;
107  unsigned int mapTimer;
108 
109  //thread interaction stuff
110  std::vector<OrientedPoint> hypotheses;
111  std::vector<unsigned int> indexes;
112  std::vector<double> weightSums;
113  pthread_mutex_t hp_mutex, ind_mutex, hist_mutex;
114  pthread_t gfs_thread;
115  bool running;
116 
117  //This are the processor parameters
118  std::string filename;
119  std::string outfilename;
120 
121  double xmin;
122  double ymin;
123  double xmax;
124  double ymax;
125  bool autosize;
126  double delta;
128 
129  //scan matching parameters
130  double sigma;
131  double maxrange;
132  double maxUrange;
133  double regscore;
134  double lstep;
135  double astep;
138  double critscore;
139  double maxMove;
140  unsigned int lskip;
141 
142  //likelihood
143  double lsigma;
144  double ogain;
145  double llsamplerange, lasamplerange;
146  double llsamplestep, lasamplestep;
149 
150 
151  //motion model parameters
152  double srr, srt, str, stt;
153  //particle parameters
156 
157  //gfs parameters
159  double linearUpdate;
160 
161  //robot config
163  //input stream
165  std::ifstream plainStream;
167  bool onLine;
170  unsigned int randseed;
171 
172  //dirty carmen interface
173  const char* const * m_argv;
174  unsigned int m_argc;
175 
176 };
177 #endif
InputSensorStream * input
Definition: gsp_thread.h:164
void setMapUpdateTime(unsigned int ut)
Definition: gsp_thread.h:97
unsigned int mapTimer
Definition: gsp_thread.h:107
std::vector< double > weightSums
Definition: gsp_thread.h:112
const char *const * m_argv
Definition: gsp_thread.h:173
std::string outfilename
Definition: gsp_thread.h:119
std::vector< unsigned int > indexes
Definition: gsp_thread.h:111
deque< Event * > EventDeque
Definition: gsp_thread.h:73
double linearOdometryReliability
Definition: gsp_thread.h:147
std::ifstream plainStream
Definition: gsp_thread.h:165
unsigned int eventBufferLength
Definition: gsp_thread.h:105
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:19
std::vector< OrientedPoint > hypotheses
Definition: gsp_thread.h:110
double angularOdometryReliability
Definition: gsp_thread.h:148
std::vector< unsigned int > indexes
Definition: gsp_thread.h:60
unsigned int mapUpdateTime
Definition: gsp_thread.h:106
std::vector< OrientedPoint > hypotheses
Definition: gsp_thread.h:51
bool isRunning() const
Definition: gsp_thread.h:98
pthread_mutex_t ind_mutex
Definition: gsp_thread.h:113
unsigned int randseed
Definition: gsp_thread.h:170


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