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;