DatabaseViewer: added exporting of a specific session with or without user_data. MainWindow: added action to send goal to rtabmap. Rtabmap: Saving goals in user_data of the current signature, modified parameter Mem/RehearsedNodesKept to Mem/NotLinkedNodesKept. DBReader: added sending goals when detected while playing back a database.

corelib/include/rtabmap/core/Memory.h

@@ -179,7 +179,7 @@ private:
 	void preUpdate();
 	void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance);
 	void clear();
-	void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);
+	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
 
 	void addSignatureToWm(Signature * signature);
 	Signature * _getSignature(int id) const;
@@ -211,7 +211,7 @@ private:
 	float _similarityThreshold;
 	bool _rawDataKept;
 	bool _binDataKept;
-	bool _keepRehearsedNodesInDb;
+	bool _notLinkedNodesKeptInDb;
 	bool _incrementalMemory;
 	int _maxStMemSize;
 	float _recentWmRatio;

corelib/include/rtabmap/core/Parameters.h

@@ -185,7 +185,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.6, 	"Rehearsal similarity.");
 	RTABMAP_PARAM(Mem, ImageKept, 		        bool, false, 	"Keep raw images in RAM.");
 	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
-	RTABMAP_PARAM(Mem, RehearsedNodesKept, 	    bool, true, 	"Keep rehearsed nodes in db.");
+	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, otherwise it is Localization mode.");
 	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2, 	"Ratio of locations after the last loop closure in WM that cannot be transferred.");
@@ -290,6 +290,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
 	RTABMAP_PARAM(RGBD, GoalReachedRadius,    float, 0.5, "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanWithNearNodesLinked,  bool, true, "Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
+	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
 	RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -111,7 +111,6 @@ public:
 	void dumpData() const;
 	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
-	void deleteLocation(int locationId); // Only nodes in STM can be deleted
 	void rejectLoopClosure(int oldId, int newId);
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
@@ -192,6 +191,7 @@ private:
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
 	bool _planWithNearNodesLinked;
+	bool _goalsSavedInUserData;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -73,7 +73,8 @@ public:
 			kCmdPublishTOROGraphGlobal, // params: optimized
 			kCmdPublishTOROGraphLocal, // params: optimized
 			kCmdTriggerNewMap,
-			kCmdPause};
+			kCmdPause,
+			kCmdGoal}; // params: label or location ID
 public:
 	RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) :
 			UEvent(0),
@@ -184,6 +185,24 @@ private:
 	std::map<int, std::vector<unsigned char> > _userDatas;
 };
 
+class RtabmapGlobalPathEvent : public UEvent
+{
+public:
+	RtabmapGlobalPathEvent():
+		UEvent(0) {}
+	RtabmapGlobalPathEvent(int goalId, const std::vector<std::pair<int, Transform> > & poses) :
+		UEvent(goalId),
+		_poses(poses) {}
+
+	virtual ~RtabmapGlobalPathEvent() {}
+	int getGoal() const {return this->getCode();}
+	const std::vector<std::pair<int, Transform> > & getPoses() const {return _poses;}
+	virtual std::string getClassName() const {return std::string("RtabmapGlobalPathEvent");}
+
+private:
+	std::vector<std::pair<int, Transform> > _poses;
+};
+
 } // namespace rtabmap
 
 #endif /* RTABMAPEVENT_H_ */

corelib/include/rtabmap/core/RtabmapThread.h

@@ -70,7 +70,8 @@ public:
 		kStatePublishingTOROGraphLocal,
 		kStatePublishingTOROGraphGlobal,
 		kStateTriggeringMap,
-		kStateAddingUserData
+		kStateAddingUserData,
+		kStateSettingGoal
 	};
 
 public:

corelib/include/rtabmap/core/SensorData.h

@@ -87,6 +87,7 @@ public:
 	int id() const {return _id;}
 	void setId(int id) {_id = id;}
 	double stamp() const {return _stamp;}
+	void setStamp(double stamp) {_stamp = stamp;}
 
 	bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
 	void setPose(const Transform & pose, float rotVariance, float transVariance) {_pose = pose; _poseRotVariance=rotVariance; _poseTransVariance = transVariance;}
@@ -114,7 +115,7 @@ public:
 	const cv::Mat & descriptors() const {return _descriptors;}
 
 	void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
-	const std::vector<unsigned char> userData() const {return _userData;}
+	const std::vector<unsigned char> & userData() const {return _userData;}
 
 private:
 	cv::Mat _image;

corelib/src/DBReader.cpp

@@ -31,8 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
 
 #include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Compression.h"
@@ -135,6 +138,23 @@ void DBReader::mainLoop()
 	SensorData data = this->getNextData();
 	if(data.isValid())
 	{
+		int goalId = 0;
+		double previousStamp = data.stamp();
+		data.setStamp(UTimer::now());
+		if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0)
+		{
+			//GOAL format detected, remove it from the user data and send it as goal event
+			std::string goalStr = uBytes2Str(data.userData());
+			if(!goalStr.empty())
+			{
+				std::list<std::string> strs = uSplit(goalStr, ':');
+				if(strs.size() == 2)
+				{
+					goalId = atoi(strs.rbegin()->c_str());
+					data.setUserData(std::vector<unsigned char>());
+				}
+			}
+		}
 		if(!_odometryIgnored)
 		{
 			if(data.pose().isNull())
@@ -150,6 +170,35 @@ void DBReader::mainLoop()
 			this->post(new CameraEvent(data));
 		}
 
+		if(goalId > 0)
+		{
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
+
+			if(_currentId != _ids.end())
+			{
+				// get stamp for the next signature to compute the delay
+				// that was used originally for planning
+				int weight;
+				std::string label;
+				double stamp;
+				int mapId;
+				Transform localTransform, pose;
+				std::vector<unsigned char> userData;
+				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
+				if(previousStamp && stamp && stamp > previousStamp)
+				{
+					double delay = stamp - previousStamp;
+					UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
+							goalId, delay);
+					uSleep(delay*1000);
+				}
+				else
+				{
+					UWARN("stamps = %d=%f %d=%f ", data.id(), data.stamp(), *_currentId, stamp);
+				}
+			}
+		}
+
 	}
 	else if(!this->isKilled())
 	{
@@ -246,7 +295,7 @@ SensorData DBReader::getNextData()
 					rotVariance,
 					transVariance,
 					seq,
-					UTimer::now(),
+					stamp,
 					userData);
 			UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
 					data.laserScan().empty()?0:1,

corelib/src/Memory.cpp

@@ -59,7 +59,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
 	_rawDataKept(Parameters::defaultMemImageKept()),
 	_binDataKept(Parameters::defaultMemBinDataKept()),
-	_keepRehearsedNodesInDb(Parameters::defaultMemRehearsedNodesKept()),
+	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_maxStMemSize(Parameters::defaultMemSTMSize()),
 	_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
@@ -379,7 +379,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 
 	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
 	Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
-	Parameters::parse(parameters, Parameters::kMemRehearsedNodesKept(), _keepRehearsedNodesInDb);
+	Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
 	Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -1499,13 +1499,13 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 /**
  * If saveToDatabase=false, deleted words are filled in deletedWords.
  */
-void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * deletedWords)
+void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> * deletedWords)
 {
 	UDEBUG("id=%d", s?s->id():0);
 	if(s)
 	{
 		// If not saved to database or it is a bad signature (not saved), remove links!
-		if(!saveToDatabase || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
+		if(!keepLinkedToGraph || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
 		{
 			UASSERT_MSG(this->isInSTM(s->id()),
 					uFormat("Deleting location (%d) outside the STM is not implemented!", s->id()).c_str());
@@ -1537,6 +1537,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
 			}
 			s->removeLinks(); // remove all links
 			s->setWeight(0);
+			s->setLabel(""); // reset label
 		}
 		else
 		{
@@ -1561,7 +1562,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
 		}
 
 		this->disableWordsRef(s->id());
-		if(!saveToDatabase)
+		if(!keepLinkedToGraph)
 		{
 			std::list<int> keys = uUniqueKeys(s->getWords());
 			for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
@@ -1599,7 +1600,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
 			}
 		}
 
-		if(	saveToDatabase &&
+		if(	(_notLinkedNodesKeptInDb || keepLinkedToGraph) &&
 			_dbDriver &&
 			s->id()>0)
 		{
@@ -2711,8 +2712,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 		}
 
 		// remove location
-		bool saveToDb = _keepRehearsedNodesInDb;
+		moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
-		moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, saveToDb);
 
 		return true;
 	}

corelib/src/Odometry.cpp

@@ -177,6 +177,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
 	customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
 	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+	customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
 	int nn = Parameters::defaultOdomBowNNType();
 	float nndr = Parameters::defaultOdomBowNNDR();
 	int featureType = Parameters::defaultOdomFeatureType();

corelib/src/Rtabmap.cpp

@@ -109,6 +109,7 @@ Rtabmap::Rtabmap() :
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
 	_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
+	_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -395,6 +396,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
 	Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
+	Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
 
 	// RGB-D SLAM stuff
 	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -1504,7 +1506,7 @@ bool Rtabmap::process(const SensorData & data)
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
-				UWARN("Rejected loop closure transform between %d and %d: %s",
+				UWARN("Rejected loop closure %d -> %d: %s",
 						_loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
 			}
 		}
@@ -2069,16 +2071,6 @@ void Rtabmap::setWorkingDirectory(std::string path)
 	}
 }
 
-
-
-void Rtabmap::deleteLocation(int locationId)
-{
-	if(_memory)
-	{
-		_memory->deleteLocation(locationId);
-	}
-}
-
 void Rtabmap::rejectLoopClosure(int oldId, int newId)
 {
 	UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
@@ -2596,6 +2588,12 @@ bool Rtabmap::computePath(
 				}
 				UINFO("Path = [%s]", stream.str().c_str());
 			}
+			if(_goalsSavedInUserData)
+			{
+				// set goal to latest signature
+				std::string goalStr = uFormat("GOAL:%d", targetNode);
+				setUserData(0, uStr2Bytes(goalStr));
+			}
 		}
 
 		return _path.size()>0;

corelib/src/RtabmapThread.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/ParamEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/UserDataEvent.h"
+#include "rtabmap/core/Memory.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
@@ -194,6 +195,7 @@ void RtabmapThread::mainLoop()
 	}
 	_stateMutex.unlock();
 
+	int id = 0;
 	std::vector<unsigned char> userData;
 	switch(state)
 	{
@@ -272,6 +274,18 @@ void RtabmapThread::mainLoop()
 		_userDataMutex.unlock();
 		_rtabmap->setUserData(0, userData);
 		break;
+	case kStateSettingGoal:
+		id = atoi(parameters.at("goal_id").c_str());
+		if(id == 0 && !parameters.at("goal_label").empty() && _rtabmap->getMemory())
+		{
+			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("goal_label"));
+		}
+		if(id <= 0 || !_rtabmap->computePath(id, true))
+		{
+			UERROR("Failed to set a goal to location=%d.", id);
+		}
+		this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath()));
+		break;
 	default:
 		UFATAL("Invalid state !?!?");
 		break;
@@ -448,6 +462,14 @@ void RtabmapThread::handleEvent(UEvent* event)
 			ULOGGER_DEBUG("CMD_PAUSE");
 			_paused = !_paused;
 		}
+		else if(cmd == RtabmapEventCmd::kCmdGoal)
+		{
+			ULOGGER_DEBUG("CMD_GOAL");
+			ParametersMap param;
+			param.insert(ParametersPair("goal_label", rtabmapEvent->getStr()));
+			param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
+			pushNewState(kStateSettingGoal, param);
+		}
 		else
 		{
 			UWARN("Cmd %d unknown!", cmd);

guilib/include/rtabmap/gui/MainWindow.h

@@ -153,6 +153,7 @@ private slots:
 	void selectStereoFlyCapture2();
 	void dumpTheMemory();
 	void dumpThePrediction();
+	void sendGoal();
 	void downloadAllClouds();
 	void downloadPoseGraph();
 	void clearTheCache();
@@ -167,6 +168,7 @@ private slots:
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters);
 	void processRtabmapEventInit(int status, const QString & info);
 	void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
+	void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event);
 	void changeImgRateSetting();
 	void changeDetectionRateSetting();
 	void changeTimeLimitSetting();
@@ -199,6 +201,7 @@ signals:
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);
 	void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
+	void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
 	void imgRateChanged(double);
 	void detectionRateChanged(double);
 	void timeLimitChanged(float);

guilib/src/DataRecorder.cpp

@@ -75,7 +75,7 @@ bool DataRecorder::init(const QString & path, bool recordInRAM)
 		customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
 		if(!recordInRAM)
 		{
-			customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); // to keep images
+			customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
 		}
 		memory_ = new Memory();
 		if(!memory_->init(path.toStdString(), true, customParameters))
@@ -125,6 +125,13 @@ void DataRecorder::addData(const rtabmap::SensorData & data)
 	memoryMutex_.lock();
 	if(memory_)
 	{
+		if(memory_->getStMem().size() == 0 && data.id() > 0)
+		{
+			ParametersMap customParameters;
+			customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); // use id from data
+			memory_->parseParameters(customParameters);
+		}
+
 		//save to database
 		UTimer time;
 		memory_->update(data);

guilib/src/DatabaseViewer.cpp

@@ -706,6 +706,10 @@ void DatabaseViewer::exportDatabase()
 
 					Signature data = memory_->getSignatureData(id, true);
 					rtabmap::SensorData sensorData = data.toSensorData();
+					if(!dialog.isUserDataExported())
+					{
+						sensorData.setUserData(std::vector<unsigned char>());
+					}
 					recorder.addData(sensorData);
 
 					progressDialog.appendText(tr("Exported node %1").arg(id));

guilib/src/ExportDialog.cpp

@@ -46,6 +46,7 @@ ExportDialog::ExportDialog(QWidget * parent) :
 	connect(_ui->checkBox_depth, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_depth2d, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_odom, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_userData, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 
 	_ui->lineEdit_path->setText(QDir::homePath()+QDir::separator()+"output.db");
 }
@@ -99,4 +100,9 @@ bool ExportDialog::isOdomExported() const
 	return _ui->checkBox_odom->isChecked();
 }
 
+bool ExportDialog::isUserDataExported() const
+{
+	return _ui->checkBox_userData->isChecked();
+}
+
 }

guilib/src/ExportDialog.h

@@ -50,6 +50,7 @@ public:
 	bool isDepthExported() const;
 	bool isDepth2dExported() const;
 	bool isOdomExported() const;
+	bool isUserDataExported() const;
 
 signals:
 	void configChanged();

guilib/src/GraphViewer.cpp

@@ -157,7 +157,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_loopClosureLocalColor(Qt::yellow),
 		_loopClosureUserColor(Qt::red),
 		_loopClosureVirtualColor(Qt::magenta),
-		_pathColor(Qt::cyan),
+		_localPathColor(Qt::cyan),
+		_globalPathColor(Qt::darkMagenta),
 		_root(0),
 		_nodeRadius(0.01),
 		_linkWidth(0),
@@ -430,6 +431,32 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
 	}
 }
 
+void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath)
+{
+	UDEBUG("Set global path size=%d", (int)globalPath.size());
+	qDeleteAll(_globalPathLinkItems);
+	_globalPathLinkItems.clear();
+
+	if(globalPath.size() >= 2)
+	{
+		for(unsigned int i=0; i<globalPath.size()-1; ++i)
+		{
+			//create a link item
+			int idFrom = globalPath[i].first;
+			int idTo = globalPath[i+1].first;
+			LinkItem * item = new LinkItem(idFrom, idTo, globalPath[i].second, globalPath[i+1].second, Link::kUndef);
+			QPen p = item->pen();
+			p.setWidthF(_linkWidth);
+			item->setPen(p);
+			item->setColor(_globalPathColor);
+			this->scene()->addItem(item);
+			item->setZValue(1);
+			item->setParentItem(_root);
+			_globalPathLinkItems.insert(idFrom, item);
+		}
+	}
+}
+
 void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
 {
 	if(localPath.size() > 1)
@@ -445,7 +472,7 @@ void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
 				{
 					if(itemIter.value()->to() == idTo)
 					{
-						itemIter.value()->setColor(_pathColor);
+						itemIter.value()->setColor(_localPathColor);
 						break;
 					}
 					++itemIter;
@@ -461,6 +488,8 @@ void GraphViewer::clearGraph()
 	_nodeItems.clear();
 	qDeleteAll(_linkItems);
 	_linkItems.clear();
+	qDeleteAll(_globalPathLinkItems);
+	_globalPathLinkItems.clear();
 	_referential->resetTransform();
 	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
 }
@@ -501,6 +530,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("user_color", this->getUserLoopClosureColor());
 	settings.setValue("virtual_color", this->getVirtualLoopClosureColor());
 	settings.setValue("local_path_color", this->getLocalPathColor());
+	settings.setValue("global_path_color", this->getGlobalPathColor());
 	settings.setValue("grid_visible", this->isGridMapVisible());
 	settings.setValue("origin_visible", this->isOriginVisible());
 	settings.setValue("referential_visible", this->isReferentialVisible());
@@ -525,6 +555,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setUserLoopClosureColor(settings.value("user_color", this->getUserLoopClosureColor()).value<QColor>());
 	this->setVirtualLoopClosureColor(settings.value("virtual_color", this->getVirtualLoopClosureColor()).value<QColor>());
 	this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
+	this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
 	this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
 	this->setOriginVisible(settings.value("origin_visible", this->isOriginVisible()).toBool());
 	this->setReferentialVisible(settings.value("referential_visible", this->isReferentialVisible()).toBool());
@@ -644,7 +675,11 @@ void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
 }
 void GraphViewer::setLocalPathColor(const QColor & color)
 {
-	_pathColor = color;
+	_localPathColor = color;
+}
+void GraphViewer::setGlobalPathColor(const QColor & color)
+{
+	_globalPathColor = color;
 }
 void GraphViewer::setGridMapVisible(bool visible)
 {
@@ -710,18 +745,21 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
 	QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
 	QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
+	QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
 	aChangeNeighborColor->setIcon(createIcon(_neighborColor));
 	aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor));
 	aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
 	aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
 	aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
-	aChangeLocalPathColor->setIcon(createIcon(_pathColor));
+	aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
+	aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
 	aChangeNeighborColor->setIconVisibleInMenu(true);
 	aChangeGlobalLoopColor->setIconVisibleInMenu(true);
 	aChangeLocalLoopColor->setIconVisibleInMenu(true);
 	aChangeUserLoopColor->setIconVisibleInMenu(true);
 	aChangeVirtualLoopColor->setIconVisibleInMenu(true);
 	aChangeLocalPathColor->setIconVisibleInMenu(true);
+	aChangeGlobalPathColor->setIconVisibleInMenu(true);
 
 	menu.addSeparator();
 	QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
@@ -830,7 +868,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			r == aChangeLocalLoopColor ||
 			r == aChangeUserLoopColor ||
 			r == aChangeVirtualLoopColor ||
-			r == aChangeLocalPathColor)
+			r == aChangeLocalPathColor ||
+			r == aChangeGlobalPathColor)
 	{
 		QColor color;
 		if(r == aChangeNodeColor)
@@ -855,7 +894,11 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		}
 		else if(r == aChangeLocalPathColor)
 		{
-			color = _pathColor;
+			color = _localPathColor;
+		}
+		else if(r == aChangeGlobalPathColor)
+		{
+			color = _globalPathColor;
 		}
 		else //if(r == aChangeNeighborColor)
 		{
@@ -902,7 +945,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	else if(r == aSetNodeSize)
 	{
 		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.01, 100, 2, &ok);
+		double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.001, 100, 3, &ok);
 		if(ok)
 		{
 			setNodeRadius(value);

guilib/src/GraphViewer.h

@@ -58,6 +58,7 @@ public:
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
 	void updatePosterior(const std::map<int, float> & posterior);
 	void updateLocalPath(const std::vector<int> & localPath);
+	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void clearGraph();
 	void clearMap();
 	void clearPosterior();
@@ -76,7 +77,8 @@ public:
 	const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
 	const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;}
 	const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;}
-	const QColor & getLocalPathColor() const {return _pathColor;}
+	const QColor & getLocalPathColor() const {return _localPathColor;}
+	const QColor & getGlobalPathColor() const {return _globalPathColor;}
 	bool isGridMapVisible() const;
 	bool isOriginVisible() const;
 	bool isReferentialVisible() const;
@@ -92,6 +94,7 @@ public:
 	void setUserLoopClosureColor(const QColor & color);
 	void setVirtualLoopClosureColor(const QColor & color);
 	void setLocalPathColor(const QColor & color);
+	void setGlobalPathColor(const QColor & color);
 	void setGridMapVisible(bool visible);
 	void setOriginVisible(bool visible);
 	void setReferentialVisible(bool visible);
@@ -114,10 +117,12 @@ private:
 	QColor _loopClosureLocalColor;
 	QColor _loopClosureUserColor;
 	QColor _loopClosureVirtualColor;
-	QColor _pathColor;
+	QColor _localPathColor;
+	QColor _globalPathColor;
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _linkItems;
+	QMultiMap<int, LinkItem*> _globalPathLinkItems;
 	float _nodeRadius;
 	float _linkWidth;
 	QGraphicsPixmapItem * _gridMap;

guilib/src/MainWindow.cpp

@@ -253,6 +253,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(this, SIGNAL(rtabmapEventInitReceived(int, const QString &)), this, SLOT(processRtabmapEventInit(int, const QString &)));
 	qRegisterMetaType<rtabmap::RtabmapEvent3DMap>("rtabmap::RtabmapEvent3DMap");
 	connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
+	qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
+	connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
 
 	// Dock Widget view actions (Menu->Window)
 	_ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
@@ -285,6 +287,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
 	connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
 	connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
+	connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
 	connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
 	connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
 	connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
@@ -635,6 +638,11 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		RtabmapEvent3DMap * rtabmapEvent3DMap = (RtabmapEvent3DMap*)anEvent;
 		emit rtabmapEvent3DMapReceived(*rtabmapEvent3DMap);
 	}
+	else if(anEvent->getClassName().compare("RtabmapGlobalPathEvent") == 0)
+	{
+		RtabmapGlobalPathEvent * rtabmapGlobalPathEvent = (RtabmapGlobalPathEvent*)anEvent;
+		emit rtabmapGlobalPathEventReceived(*rtabmapGlobalPathEvent);
+	}
 	else if(anEvent->getClassName().compare("CameraEvent") == 0)
 	{
 		CameraEvent * cameraEvent = (CameraEvent*)anEvent;
@@ -744,6 +752,8 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 	{
 		_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize);
 	}
+	_ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id());
+
 	float x,y,z, roll,pitch,yaw;
 	pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 	_ui->statsToolBox->updateStat("Odometry/T_x/m", (float)data.id(), x);
@@ -1183,6 +1193,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		}
 		// update local path on the graph view
 		_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
+		if(stat.localPath().size() == 0)
+		{
+			// clear the global path if set (goal reached)
+			_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
+		}
 
 		UDEBUG("");
 	}
@@ -1921,6 +1936,22 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 	_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
 }
 
+void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
+{
+	if(event.getPoses().empty())
+	{
+		QMessageBox::warning(this, tr("Setting goal failed"), tr("Setting goal to location %1 failed. "
+				"Some reasons: \n"
+				"1) the location doesn't exist in the graph,\n"
+				"2) the location is not linked to the global graph or\n"
+				"3) the location is too near of the current location (goal already reached).").arg(event.getGoal()));
+	}
+	else
+	{
+		_ui->graphicsView_graphView->setGlobalPath(event.getPoses());
+	}
+}
+
 void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 {
 	ULOGGER_DEBUG("");
@@ -3594,6 +3625,19 @@ void MainWindow::dumpThePrediction()
 	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
 }
 
+void MainWindow::sendGoal()
+{
+	UINFO("Sending a goal...");
+	bool ok = false;
+	int id = QInputDialog::getInt(this, tr("Send a goal"), tr("Goal location ID: "), 1, 1, 99999, 1, &ok);
+	if(ok)
+	{
+		_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
+		UINFO("Posting event with goal %d", id);
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", id));
+	}
+}
+
 void MainWindow::downloadAllClouds()
 {
 	QStringList items;

guilib/src/PreferencesDialog.cpp

@@ -349,7 +349,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// Memory
 	_ui->general_checkBox_keepRawData->setObjectName(Parameters::kMemImageKept().c_str());
 	_ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str());
-	_ui->general_checkBox_keepRehearsedNodes->setObjectName(Parameters::kMemRehearsedNodesKept().c_str());
+	_ui->general_checkBox_keepNotLinkedNodes->setObjectName(Parameters::kMemNotLinkedNodesKept().c_str());
 	_ui->general_spinBox_maxStMemSize->setObjectName(Parameters::kMemSTMSize().c_str());
 	_ui->doubleSpinBox_similarityThreshold->setObjectName(Parameters::kMemRehearsalSimilarity().c_str());
 	_ui->general_checkBox_SLAM_mode->setObjectName(Parameters::kMemIncrementalMemory().c_str());
@@ -476,6 +476,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanWithNearNodesLinked().c_str());
+	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
 
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());

guilib/src/ui/exportDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>371</width>
+    <width>382</width>
-    <height>236</height>
+    <height>291</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -125,6 +125,16 @@
      </property>
     </widget>
    </item>
+   <item>
+    <widget class="QCheckBox" name="checkBox_userData">
+     <property name="text">
+      <string>User data (planned goals)</string>
+     </property>
+     <property name="checked">
+      <bool>false</bool>
+     </property>
+    </widget>
+   </item>
    <item>
     <layout class="QHBoxLayout" name="horizontalLayout">
      <item>

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>21</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -61,6 +61,7 @@
      <addaction name="actionGenerate_local_map"/>
      <addaction name="actionGenerate_TORO_graph_graph"/>
      <addaction name="actionPrint_loop_closure_IDs_to_console"/>
+     <addaction name="actionSend_goal"/>
     </widget>
     <addaction name="actionDownload_all_clouds"/>
     <addaction name="actionDownload_graph"/>
@@ -1215,6 +1216,11 @@
     <string>StereoFlyCapture2</string>
    </property>
   </action>
+  <action name="actionSend_goal">
+   <property name="text">
+    <string>Send a goal...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1035</width>
-    <height>716</height>
+    <height>702</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>736</width>
+            <width>737</width>
-            <height>781</height>
+            <height>948</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>0</number>
+                <number>8</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -3140,7 +3140,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                     <item row="1" column="1">
                      <widget class="QLabel" name="label_keepRehearsed">
                       <property name="text">
-                       <string>Keep rehearsed locations.</string>
+                       <string>Keep not linked nodes in db (rehearsed nodes and deleted nodes are saved).</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>
@@ -3148,7 +3148,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                      </widget>
                     </item>
                     <item row="1" column="0">
-                     <widget class="QCheckBox" name="general_checkBox_keepRehearsedNodes">
+                     <widget class="QCheckBox" name="general_checkBox_keepNotLinkedNodes">
                       <property name="text">
                        <string/>
                       </property>
@@ -3280,7 +3280,7 @@ generate the number of words requested.</string>
                       <item row="2" column="0">
                        <widget class="QSpinBox" name="surf_spinBox_wordsPerImageTarget">
                         <property name="minimum">
-                         <number>0</number>
+                         <number>-1</number>
                         </property>
                         <property name="maximum">
                          <number>2000</number>
@@ -3299,7 +3299,7 @@ Otherwise, the threshold is modified to
 generate the number of words requested.</string>
                         </property>
                         <property name="text">
-                         <string>Maximum words per image.</string>
+                         <string>Maximum words per image (0=no maximum). Setting to -1 will disable features extraction, so disabling loop closure detection indirectly.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -5290,6 +5290,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_space3_5">
+                           <property name="text">
+                            <string>When a goal is received and processed with success, it is saved in user data of the location with this format: &quot;GOAL:#&quot;.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="0">
+                          <widget class="QCheckBox" name="graphPlan_goalsSavedInUserData">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>

utilite/include/rtabmap/utilite/UConversion.h

@@ -149,6 +149,20 @@ std::string UTILITE_EXP uBool2Str(bool boolean);
  */
 bool UTILITE_EXP uStr2Bool(const char * str);
 
+/**
+ * Convert a string to an array of bytes including the null character ('\0').
+ * @param str the string
+ * @return the array of bytes
+ */
+std::vector<unsigned char> UTILITE_EXP uStr2Bytes(const std::string & str);
+
+/**
+ * Convert an array of bytes to string, the array of bytes must end with the null character ('\0').
+ * @param bytes the array of bytes
+ * @return the string
+ */
+std::string UTILITE_EXP uBytes2Str(const std::vector<unsigned char> & bytes);
+
 /**
  * Convert a bytes array to an hexadecimal string.
  * The resulting string is twice the size of the bytes array. The hexadecimal

utilite/src/UConversion.cpp

@@ -150,6 +150,29 @@ bool uStr2Bool(const char * str)
 	return !(str && (strcmp(str, "false") == 0 || strcmp(str, "FALSE") == 0 || strcmp(str, "0") == 0));
 }
 
+std::vector<unsigned char> uStr2Bytes(const std::string & str)
+{
+	std::vector<unsigned char> bytes(str.size()+1);
+	memcpy(bytes.data(), str.data(), str.size());
+	bytes[bytes.size()-1] = '\0'; // null character
+	return bytes;
+}
+
+std::string uBytes2Str(const std::vector<unsigned char> & bytes)
+{
+	if(bytes.size())
+	{
+		if(bytes[bytes.size()-1] != '\0')
+		{
+			std::vector<unsigned char> tmp = bytes;
+			tmp.push_back('\0');
+			return std::string((const char *)tmp.data());
+		}
+		return std::string((const char *)bytes.data());
+	}
+	return std::string();
+}
+
 std::string uBytes2Hex(const char * bytes, unsigned int bytesLen)
 {
 	std::string hex;