gsp_thread.h

Go to the documentation of this file.

/*****************************************************************
 *
 * 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 3-Clause BSD License
 * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
 * and Wolfram Burgard.
 * 
 * Further information on this license can be found at:
 * https://opensource.org/licenses/BSD-3-Clause
 * 
 * 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.  
 *
 *****************************************************************/


#ifndef GSP_THREAD_H
#define GSP_THREAD_H

#ifndef _WIN32
  #include <unistd.h>
  #include <pthread.h>
#endif
#include <deque>
#include <fstream>
#include <iostream>
#include <gmapping/log/carmenconfiguration.h>
#include <gmapping/log/sensorstream.h>
#include <gmapping/gridfastslam/gridslamprocessor.h>

using namespace std;
using namespace GMapping;


#define MAX_STRING_LENGTH 1024


struct GridSlamProcessorThread : public GridSlamProcessor {
                struct Event{
                        virtual ~Event();
                };
                
                struct ParticleMoveEvent: public Event{
                        bool scanmatched;
                        double neff;
                        std::vector<OrientedPoint> hypotheses;
                        std::vector<double> weightSums;
                };
                
                struct TruePosEvent : public Event{
                        OrientedPoint pose;
                };
                
                struct ResampleEvent: public Event{
                        std::vector<unsigned int> indexes;
                };
                
                struct MapEvent: public Event{
                        ScanMatcherMap* pmap;
                        unsigned int index;
                        OrientedPoint pose;
                        virtual ~MapEvent();
                };
                
                struct DoneEvent: public Event{
                };
                
                typedef deque<Event*> EventDeque;
                
                GridSlamProcessorThread();
                ~GridSlamProcessorThread();     
                int init(int argc, const char * const * argv);
                int loadFiles(const char * fn=0);
                static void * fastslamthread(GridSlamProcessorThread* gpt);
                std::vector<OrientedPoint> getHypotheses();
                std::vector<unsigned int> getIndexes();
                
                EventDeque getEvents();
                
                void start();
                void stop();
                
                virtual void onOdometryUpdate();
                virtual void onResampleUpdate();
                virtual void onScanmatchUpdate();
                
                virtual void syncOdometryUpdate();
                virtual void syncResampleUpdate();
                virtual void syncScanmatchUpdate();
                
                void setEventBufferSize(unsigned int length);
                inline void setMapUpdateTime(unsigned int ut) {mapUpdateTime=ut;}
                inline bool isRunning() const {return running;}
                OrientedPoint boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const;
        private:
                
                void addEvent(Event *);
                EventDeque eventBuffer;
                
                unsigned int eventBufferLength;
                unsigned int mapUpdateTime;
                unsigned int mapTimer;
                
                //thread interaction stuff
                std::vector<OrientedPoint> hypotheses;
                std::vector<unsigned int> indexes;
                std::vector<double> weightSums;
                pthread_mutex_t hp_mutex, ind_mutex, hist_mutex;
                pthread_t gfs_thread;
                bool running;
                
                //This are the processor parameters
                std::string filename;
                std::string outfilename;

                double xmin;
                double ymin;
                double xmax;
                double ymax;
                bool autosize;
                double delta;
                double resampleThreshold;
                
                //scan matching parameters
                double sigma;
                double maxrange;
                double maxUrange;
                double regscore;
                double lstep;
                double astep;
                int kernelSize;
                int iterations;
                double critscore;
                double maxMove;
                unsigned int lskip;

                //likelihood
                double lsigma;
                double ogain;
                double llsamplerange, lasamplerange;
                double llsamplestep, lasamplestep;
                double linearOdometryReliability;
                double angularOdometryReliability;
                
        
                //motion model parameters
                double srr, srt, str, stt;
                //particle parameters
                int particles;
                bool skipMatching;
                
                //gfs parameters
                double angularUpdate;
                double linearUpdate;
        
                //robot config
                SensorMap sensorMap;
                //input stream
                InputSensorStream* input;
                std::ifstream plainStream;
                bool readFromStdin;
                bool onLine;
                bool generateMap;
                bool considerOdometryCovariance;
                unsigned int randseed;
                
                //dirty carmen interface
                const char* const * m_argv;
                unsigned int m_argc;
                
};
#endif