Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef RTABMAPTHREAD_H_
00029 #define RTABMAPTHREAD_H_
00030
00031 #include "rtabmap/core/RtabmapExp.h"
00032
00033 #include <rtabmap/utilite/UThreadNode.h>
00034 #include <rtabmap/utilite/UEventsHandler.h>
00035 #include <rtabmap/utilite/USemaphore.h>
00036 #include <rtabmap/utilite/UMutex.h>
00037
00038 #include "rtabmap/core/RtabmapEvent.h"
00039 #include "rtabmap/core/SensorData.h"
00040 #include "rtabmap/core/Parameters.h"
00041 #include "rtabmap/core/OdometryEvent.h"
00042
00043 #include <stack>
00044
00045 class UTimer;
00046
00047 namespace rtabmap {
00048
00049 class Rtabmap;
00050
00051 class RTABMAP_EXP RtabmapThread :
00052 public UThreadNode,
00053 public UEventsHandler
00054 {
00055 public:
00056 enum State {
00057 kStateInit,
00058 kStateDetecting,
00059 kStateReseting,
00060 kStateClose,
00061 kStateChangingParameters,
00062 kStateDumpingMemory,
00063 kStateDumpingPrediction,
00064 kStateExportingDOTGraph,
00065 kStateExportingPoses,
00066 kStateCleanDataBuffer,
00067 kStatePublishingMap,
00068 kStateTriggeringMap,
00069 kStateAddingUserData,
00070 kStateSettingGoal,
00071 kStateCancellingGoal,
00072 kStateLabelling
00073 };
00074
00075 public:
00076
00077 RtabmapThread(Rtabmap * rtabmap);
00078 virtual ~RtabmapThread();
00079
00080 void clearBufferedData();
00081 void setDetectorRate(float rate);
00082 void setDataBufferSize(unsigned int bufferSize);
00083 void createIntermediateNodes(bool enabled);
00084
00085
00086 void close(bool databaseSaved);
00087
00088 protected:
00089 virtual void handleEvent(UEvent * anEvent);
00090
00091 private:
00092 virtual void mainLoopBegin();
00093 virtual void mainLoop();
00094 virtual void mainLoopKill();
00095 void process();
00096 void addData(const OdometryEvent & odomEvent);
00097 bool getData(OdometryEvent & data);
00098 void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
00099 void publishMap(bool optimized, bool full, bool graphOnly) const;
00100
00101 private:
00102 UMutex _stateMutex;
00103 std::stack<State> _state;
00104 std::stack<ParametersMap> _stateParam;
00105
00106 std::list<OdometryEvent> _dataBuffer;
00107 UMutex _dataMutex;
00108 USemaphore _dataAdded;
00109 unsigned int _dataBufferMaxSize;
00110 float _rate;
00111 bool _createIntermediateNodes;
00112 UTimer * _frameRateTimer;
00113 double _previousStamp;
00114
00115 Rtabmap * _rtabmap;
00116 bool _paused;
00117 Transform lastPose_;
00118 double _rotVariance;
00119 double _transVariance;
00120
00121 cv::Mat _userData;
00122 UMutex _userDataMutex;
00123 };
00124
00125 }
00126 #endif