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 3-Clause BSD License
9  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
10  * and Wolfram Burgard.
11  *
12  * Further information on this license can be found at:
13  * https://opensource.org/licenses/BSD-3-Clause
14  *
15  * GMAPPING is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied
17  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
18  * PURPOSE.
19  *
20  *****************************************************************/
21 
22 
23 #ifndef GSP_THREAD_H
24 #define GSP_THREAD_H
25 
26 #ifndef _WIN32
27  #include <unistd.h>
28  #include <pthread.h>
29 #endif
30 #include <deque>
31 #include <fstream>
32 #include <iostream>
36 
37 using namespace std;
38 using namespace GMapping;
39 
40 
41 #define MAX_STRING_LENGTH 1024
42 
43 
45  struct Event{
46  virtual ~Event();
47  };
48 
49  struct ParticleMoveEvent: public Event{
51  double neff;
52  std::vector<OrientedPoint> hypotheses;
53  std::vector<double> weightSums;
54  };
55 
56  struct TruePosEvent : public Event{
58  };
59 
60  struct ResampleEvent: public Event{
61  std::vector<unsigned int> indexes;
62  };
63 
64  struct MapEvent: public Event{
66  unsigned int index;
68  virtual ~MapEvent();
69  };
70 
71  struct DoneEvent: public Event{
72  };
73 
74  typedef deque<Event*> EventDeque;
75 
78  int init(int argc, const char * const * argv);
79  int loadFiles(const char * fn=0);
80  static void * fastslamthread(GridSlamProcessorThread* gpt);
81  std::vector<OrientedPoint> getHypotheses();
82  std::vector<unsigned int> getIndexes();
83 
84  EventDeque getEvents();
85 
86  void start();
87  void stop();
88 
89  virtual void onOdometryUpdate();
90  virtual void onResampleUpdate();
91  virtual void onScanmatchUpdate();
92 
93  virtual void syncOdometryUpdate();
94  virtual void syncResampleUpdate();
95  virtual void syncScanmatchUpdate();
96 
97  void setEventBufferSize(unsigned int length);
98  inline void setMapUpdateTime(unsigned int ut) {mapUpdateTime=ut;}
99  inline bool isRunning() const {return running;}
100  OrientedPoint boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const;
101  private:
102 
103  void addEvent(Event *);
105 
106  unsigned int eventBufferLength;
107  unsigned int mapUpdateTime;
108  unsigned int mapTimer;
109 
110  //thread interaction stuff
111  std::vector<OrientedPoint> hypotheses;
112  std::vector<unsigned int> indexes;
113  std::vector<double> weightSums;
114  pthread_mutex_t hp_mutex, ind_mutex, hist_mutex;
115  pthread_t gfs_thread;
116  bool running;
117 
118  //This are the processor parameters
119  std::string filename;
120  std::string outfilename;
121 
122  double xmin;
123  double ymin;
124  double xmax;
125  double ymax;
126  bool autosize;
127  double delta;
129 
130  //scan matching parameters
131  double sigma;
132  double maxrange;
133  double maxUrange;
134  double regscore;
135  double lstep;
136  double astep;
139  double critscore;
140  double maxMove;
141  unsigned int lskip;
142 
143  //likelihood
144  double lsigma;
145  double ogain;
146  double llsamplerange, lasamplerange;
147  double llsamplestep, lasamplestep;
150 
151 
152  //motion model parameters
153  double srr, srt, str, stt;
154  //particle parameters
157 
158  //gfs parameters
160  double linearUpdate;
161 
162  //robot config
164  //input stream
166  std::ifstream plainStream;
168  bool onLine;
171  unsigned int randseed;
172 
173  //dirty carmen interface
174  const char* const * m_argv;
175  unsigned int m_argc;
176 
177 };
178 #endif
GridSlamProcessorThread::astep
double astep
Definition: gsp_thread.h:136
GMapping::SensorMap
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:20
GridSlamProcessorThread::angularOdometryReliability
double angularOdometryReliability
Definition: gsp_thread.h:149
GridSlamProcessorThread::ParticleMoveEvent::hypotheses
std::vector< OrientedPoint > hypotheses
Definition: gsp_thread.h:52
GridSlamProcessorThread::sigma
double sigma
Definition: gsp_thread.h:131
GridSlamProcessorThread::setMapUpdateTime
void setMapUpdateTime(unsigned int ut)
Definition: gsp_thread.h:98
GridSlamProcessorThread::eventBuffer
EventDeque eventBuffer
Definition: gsp_thread.h:104
GridSlamProcessorThread::gfs_thread
pthread_t gfs_thread
Definition: gsp_thread.h:115
GridSlamProcessorThread::hypotheses
std::vector< OrientedPoint > hypotheses
Definition: gsp_thread.h:111
GridSlamProcessorThread::critscore
double critscore
Definition: gsp_thread.h:139
GridSlamProcessorThread::llsamplestep
double llsamplestep
Definition: gsp_thread.h:147
GridSlamProcessorThread::maxMove
double maxMove
Definition: gsp_thread.h:140
GMapping::InputSensorStream
Definition: sensorstream.h:24
GridSlamProcessorThread::ymax
double ymax
Definition: gsp_thread.h:125
sensorstream.h
GridSlamProcessorThread::TruePosEvent::pose
OrientedPoint pose
Definition: gsp_thread.h:57
GridSlamProcessorThread::mapTimer
unsigned int mapTimer
Definition: gsp_thread.h:108
GridSlamProcessorThread::kernelSize
int kernelSize
Definition: gsp_thread.h:137
GridSlamProcessorThread
Definition: gsp_thread.h:44
carmenconfiguration.h
GridSlamProcessorThread::TruePosEvent
Definition: gsp_thread.h:56
GMapping
Definition: configfile.cpp:34
GridSlamProcessorThread::autosize
bool autosize
Definition: gsp_thread.h:126
GridSlamProcessorThread::readFromStdin
bool readFromStdin
Definition: gsp_thread.h:167
GridSlamProcessorThread::m_argv
const char *const * m_argv
Definition: gsp_thread.h:174
GridSlamProcessorThread::DoneEvent
Definition: gsp_thread.h:71
GridSlamProcessorThread::stt
double stt
Definition: gsp_thread.h:153
GridSlamProcessorThread::onLine
bool onLine
Definition: gsp_thread.h:168
GridSlamProcessorThread::xmax
double xmax
Definition: gsp_thread.h:124
GridSlamProcessorThread::ResampleEvent::indexes
std::vector< unsigned int > indexes
Definition: gsp_thread.h:61
GridSlamProcessorThread::particles
int particles
Definition: gsp_thread.h:155
GridSlamProcessorThread::eventBufferLength
unsigned int eventBufferLength
Definition: gsp_thread.h:106
GridSlamProcessorThread::resampleThreshold
double resampleThreshold
Definition: gsp_thread.h:128
GridSlamProcessorThread::maxUrange
double maxUrange
Definition: gsp_thread.h:133
GMapping::Map< PointAccumulator, HierarchicalArray2D< PointAccumulator > >
GridSlamProcessorThread::ParticleMoveEvent::weightSums
std::vector< double > weightSums
Definition: gsp_thread.h:53
GridSlamProcessorThread::considerOdometryCovariance
bool considerOdometryCovariance
Definition: gsp_thread.h:170
GridSlamProcessorThread::ParticleMoveEvent::scanmatched
bool scanmatched
Definition: gsp_thread.h:50
GridSlamProcessorThread::generateMap
bool generateMap
Definition: gsp_thread.h:169
GridSlamProcessorThread::sensorMap
SensorMap sensorMap
Definition: gsp_thread.h:163
GridSlamProcessorThread::MapEvent::pose
OrientedPoint pose
Definition: gsp_thread.h:67
GridSlamProcessorThread::llsamplerange
double llsamplerange
Definition: gsp_thread.h:146
GMapping::GridSlamProcessor
Definition: gridslamprocessor.h:35
GridSlamProcessorThread::lstep
double lstep
Definition: gsp_thread.h:135
GridSlamProcessorThread::lskip
unsigned int lskip
Definition: gsp_thread.h:141
GridSlamProcessorThread::xmin
double xmin
Definition: gsp_thread.h:122
GridSlamProcessorThread::MapEvent::index
unsigned int index
Definition: gsp_thread.h:66
GridSlamProcessorThread::ResampleEvent
Definition: gsp_thread.h:60
GridSlamProcessorThread::m_argc
unsigned int m_argc
Definition: gsp_thread.h:175
GridSlamProcessorThread::EventDeque
deque< Event * > EventDeque
Definition: gsp_thread.h:74
GridSlamProcessorThread::randseed
unsigned int randseed
Definition: gsp_thread.h:171
GridSlamProcessorThread::running
bool running
Definition: gsp_thread.h:116
GridSlamProcessorThread::ParticleMoveEvent
Definition: gsp_thread.h:49
GridSlamProcessorThread::delta
double delta
Definition: gsp_thread.h:127
GridSlamProcessorThread::isRunning
bool isRunning() const
Definition: gsp_thread.h:99
GridSlamProcessorThread::iterations
int iterations
Definition: gsp_thread.h:138
GridSlamProcessorThread::filename
std::string filename
Definition: gsp_thread.h:119
GridSlamProcessorThread::linearOdometryReliability
double linearOdometryReliability
Definition: gsp_thread.h:148
GridSlamProcessorThread::outfilename
std::string outfilename
Definition: gsp_thread.h:120
GridSlamProcessorThread::input
InputSensorStream * input
Definition: gsp_thread.h:165
GridSlamProcessorThread::ind_mutex
pthread_mutex_t ind_mutex
Definition: gsp_thread.h:114
GridSlamProcessorThread::ogain
double ogain
Definition: gsp_thread.h:145
GridSlamProcessorThread::regscore
double regscore
Definition: gsp_thread.h:134
GridSlamProcessorThread::skipMatching
bool skipMatching
Definition: gsp_thread.h:156
GridSlamProcessorThread::ParticleMoveEvent::neff
double neff
Definition: gsp_thread.h:51
GridSlamProcessorThread::lsigma
double lsigma
Definition: gsp_thread.h:144
GridSlamProcessorThread::MapEvent::pmap
ScanMatcherMap * pmap
Definition: gsp_thread.h:65
gridslamprocessor.h
GridSlamProcessorThread::MapEvent
Definition: gsp_thread.h:64
GMapping::orientedpoint< double, double >
GridSlamProcessorThread::mapUpdateTime
unsigned int mapUpdateTime
Definition: gsp_thread.h:107
GridSlamProcessorThread::angularUpdate
double angularUpdate
Definition: gsp_thread.h:159
GMapping::SensorLog
Definition: sensorlog.h:16
GridSlamProcessorThread::indexes
std::vector< unsigned int > indexes
Definition: gsp_thread.h:112
GridSlamProcessorThread::linearUpdate
double linearUpdate
Definition: gsp_thread.h:160
GridSlamProcessorThread::maxrange
double maxrange
Definition: gsp_thread.h:132
GridSlamProcessorThread::Event
Definition: gsp_thread.h:45
GridSlamProcessorThread::weightSums
std::vector< double > weightSums
Definition: gsp_thread.h:113
GridSlamProcessorThread::plainStream
std::ifstream plainStream
Definition: gsp_thread.h:166
GridSlamProcessorThread::ymin
double ymin
Definition: gsp_thread.h:123


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51