Protected public rtabmap methods with a mutex (to be safe to call when rtabmap thread is started)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@734 f169173b-cf89-36c8-b27e-44dbe73f0c83

app/src/main.cpp

@@ -46,6 +46,7 @@ int main(int argc, char* argv[])
 	mainWindow->showNormal();
 
 	Rtabmap * rtabmap = new Rtabmap();
+	rtabmap->setWorkingDirectory(mainWindow->getWorkingDirectory().toStdString());
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui
 	UEventsManager::addHandler(rtabmap);
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -85,18 +85,24 @@ public:
 	const std::string & getWorkingDir() const {return _wDir;}
 	int getLoopClosureId() const;
 	int getRetrievedId() const;
-	int getLastLocationId() const;
+	int getLastLocationId();
 	float getLcHypValue() const {return _lastLcHypothesisValue;}
-	std::list<int> getWM() const; // working memory
+	std::list<int> getWM(); // working memory
-	std::set<int> getSTM() const; // short-term memory
+	std::set<int> getSTM(); // short-term memory
-	int getWMSize() const; // working memory size
+	int getWMSize(); // working memory size
-	int getSTMSize() const; // short-term memory size
+	int getSTMSize(); // short-term memory size
-	std::map<int, int> getWeights() const;
+	std::map<int, int> getWeights();
-	int getTotalMemSize() const;
+	int getTotalMemSize();
 	double getLastProcessTime() const {return _lastProcessTime;};
 
 	void setTimeThreshold(float maxTimeAllowed); // in ms
-	void generateGraph(const std::string & path) const;
+
+	void generateGraph(const std::string & path);
+	void resetMemory(bool dbOverwritten = false);
+	void dumpPrediction();
+	void dumpData();
+	void updateParameters(const ParametersMap & parameters);
+	void setWorkingDirectory(std::string path);
 
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
@@ -110,18 +116,14 @@ private:
 	virtual void mainLoopKill();
 	virtual void mainLoopBegin();
 	void process();
-	void resetMemory(bool dbOverwritten = false);
 	void addImage(const Image & image);
 	void getImage(Image & image);
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
 	void releaseAllStrategies();
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
-	void dumpPrediction() const;
-	void dumpData();
 	void generateLocalGraph(const std::string & path, int id, int margin);
 	void parseParameters(const ParametersMap & parameters);
-	void setWorkingDirectory(std::string path);
 	void setDataBufferSize(int size);
 
 private:
@@ -154,6 +156,8 @@ private:
 	UMutex _imageMutex;
 	USemaphore _imageAdded;
 
+	UMutex _threadMutex;
+
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
 	EpipolarGeometry * _epipolarGeometry;

corelib/src/Rtabmap.cpp

@@ -85,25 +85,7 @@ Rtabmap::Rtabmap() :
 
 Rtabmap::~Rtabmap() {
 	UDEBUG("");
-
+	this->close();
-	UEventsManager::removeHandler(this);
-
-	// Stop the thread first
-	join(true);
-
-	flushStatisticLogs();
-	if(_foutFloat)
-	{
-		fclose(_foutFloat);
-		_foutFloat = 0;
-	}
-	if(_foutInt)
-	{
-		fclose(_foutInt);
-		_foutInt = 0;
-	}
-
-	this->releaseAllStrategies();
 }
 
 std::string Rtabmap::getVersion()
@@ -255,6 +237,7 @@ void Rtabmap::pushNewState(State newState, const ParametersMap & parameters)
 
 void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
 {
+	UScopeMutex s(&_threadMutex);
 	if(this->isRunning())
 	{
 		pushNewState(kStateChangingParameters, parameters);
@@ -267,6 +250,12 @@ void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
 	{
 		if(deleteMemory)
 		{
+			// Set the working directory before
+			ParametersMap::const_iterator iter;
+			if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
+			{
+				this->setWorkingDirectory(iter->second.c_str());
+			}
 			this->resetMemory(true);
 		}
 
@@ -277,6 +266,7 @@ void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
 
 void Rtabmap::init(const std::string & configFile, bool deleteMemory)
 {
+	UScopeMutex s(&_threadMutex);
 	// fill ctrl struct with values from the configuration file
 	ParametersMap param;// = Parameters::defaultParameters;
 
@@ -430,8 +420,9 @@ int Rtabmap::getRetrievedId() const
 	return _retrievedId;
 }
 
-int Rtabmap::getLastLocationId() const
+int Rtabmap::getLastLocationId()
 {
+	UScopeMutex s(&_threadMutex);
 	int id = 0;
 	if(_memory && _memory->getLastSignature())
 	{
@@ -440,8 +431,9 @@ int Rtabmap::getLastLocationId() const
 	return id;
 }
 
-std::list<int> Rtabmap::getWM() const
+std::list<int> Rtabmap::getWM()
 {
+	UScopeMutex s(&_threadMutex);
 	std::list<int> mem;
 	if(_memory)
 	{
@@ -451,8 +443,9 @@ std::list<int> Rtabmap::getWM() const
 	return mem;
 }
 
-int Rtabmap::getWMSize() const
+int Rtabmap::getWMSize()
 {
+	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		return _memory->getWorkingMem().size()-1; // remove virtual place
@@ -460,8 +453,9 @@ int Rtabmap::getWMSize() const
 	return 0;
 }
 
-std::map<int, int> Rtabmap::getWeights() const
+std::map<int, int> Rtabmap::getWeights()
 {
+	UScopeMutex s(&_threadMutex);
 	std::map<int, int> weights;
 	if(_memory)
 	{
@@ -471,8 +465,9 @@ std::map<int, int> Rtabmap::getWeights() const
 	return weights;
 }
 
-std::set<int> Rtabmap::getSTM() const
+std::set<int> Rtabmap::getSTM()
 {
+	UScopeMutex s(&_threadMutex);
 	std::set<int> mem;
 	if(_memory)
 	{
@@ -481,8 +476,9 @@ std::set<int> Rtabmap::getSTM() const
 	return mem;
 }
 
-int Rtabmap::getSTMSize() const
+int Rtabmap::getSTMSize()
 {
+	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		return _memory->getStMem().size();
@@ -490,8 +486,9 @@ int Rtabmap::getSTMSize() const
 	return 0;
 }
 
-int Rtabmap::getTotalMemSize() const
+int Rtabmap::getTotalMemSize()
 {
+	UScopeMutex s(&_threadMutex);
 	ULOGGER_DEBUG("");
 	int memSize = 0;
 	if(_memory)
@@ -534,6 +531,8 @@ void Rtabmap::mainLoopKill()
 
 void Rtabmap::mainLoop()
 {
+	UScopeMutex s(&_threadMutex);
+
 	State state = kStateDetecting;
 	ParametersMap parameters;
 
@@ -598,16 +597,19 @@ void Rtabmap::mainLoop()
 	}
 }
 
-void Rtabmap::generateGraph(const std::string & path) const
+void Rtabmap::generateGraph(const std::string & path)
 {
-	if(!this->isRunning() && _memory)
+	UScopeMutex s(&_threadMutex);
+	if(_memory)
 	{
+		_memory->joinTrashThread(); // make sure the trash is flushed
 		_memory->generateGraph(path);
 	}
 }
 
 void Rtabmap::resetMemory(bool dbOverwritten)
 {
+	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
@@ -1366,7 +1368,7 @@ void Rtabmap::addImage(const Image & image)
 	_imageMutex.lock();
 	{
 		_imageBuffer.push_back(image);
-		while(_imageBufferMaxSize > 0 && _imageBuffer.size() >= (unsigned int)_imageBufferMaxSize)
+		while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
 		{
 			ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
 			_imageBuffer.pop_front();
@@ -1431,6 +1433,7 @@ void Rtabmap::setDataBufferSize(int size)
 
 void Rtabmap::setWorkingDirectory(std::string path)
 {
+	UScopeMutex scopeMutex(&_threadMutex);
 	if(path.size() && (path.at(path.size()-1) != '\\' || path.at(path.size()-1) != '/' ))
 	{
 		path += UDirectory::separator();
@@ -1465,11 +1468,13 @@ void Rtabmap::setWorkingDirectory(std::string path)
 
 void Rtabmap::process(const cv::Mat & image)
 {
+	UScopeMutex s(&_threadMutex);
 	this->process(Image(image));
 }
 
 void Rtabmap::process(const Image & image)
 {
+	UScopeMutex s(&_threadMutex);
 	if(!this->isRunning())
 	{
 		this->addImage(image);
@@ -1477,18 +1482,25 @@ void Rtabmap::process(const Image & image)
 	}
 	else
 	{
-		UERROR("The core thread is running!");
+		UERROR("The rtabmap thread is running! Don't start rtabmap thread if you want to use this method directly.");
 	}
 }
 
 void Rtabmap::dumpData()
 {
+	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		_memory->dumpMemory(this->getWorkingDir());
 	}
 }
 
+void Rtabmap::updateParameters(const ParametersMap & parameters)
+{
+	UScopeMutex s(&_threadMutex);
+	this->parseParameters(parameters);
+}
+
 
 void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 {
@@ -1560,8 +1572,9 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 	UDEBUG("mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time);
 }
 
-void Rtabmap::dumpPrediction() const
+void Rtabmap::dumpPrediction()
 {
+	UScopeMutex s(&_threadMutex);
 	if(_memory && _bayesFilter)
 	{
 		const std::set<int> & wm = _memory->getWorkingMem();