Updated how RtabmapCmdEvent should be called (using UVariant). MainWindow: added new actions to export poses (KITTI, RGBD-SLAM and TORO formats)

corelib/include/rtabmap/core/Rtabmap.h

@@ -108,15 +108,15 @@ public:
 	bool labelLocation(int id, const std::string & label);
 	bool setUserData(int id, const cv::Mat & data);
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
-	void generateTOROGraph(const std::string & path, bool optimized, bool global);
-	void exportPoses(const std::string & path, bool optimized, bool global);
+	void exportPoses(
+			const std::string & path,
+			bool optimized,
+			bool global,
+			int type // 0=raw/KITTI format, 1=rgbd-slam format, 2=TORO
+	);
 	void resetMemory();
 	void dumpPrediction() const;
 	void dumpData() const;
-	void dumpPoses(
-			const std::string & path,
-			const std::map<int, Transform> & poses,
-			const std::map<int, double> & stamps = std::map<int, double>()) const;
 	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
 	void rejectLoopClosure(int oldId, int newId);

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 #include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UVariant.h>
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Parameters.h"
 
@@ -58,53 +59,72 @@ class RtabmapEventCmd : public UEvent
 public:
 	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
 	enum Cmd {
-			kCmdInit,
+			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
 			kCmdClose,
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
-			kCmdGenerateDOTGraph, // params: path
-			kCmdGenerateDOTLocalGraph, // params: path, id, margin
-			kCmdGenerateTOROGraphLocal, // params: path, optimized
-			kCmdGenerateTOROGraphGlobal, // params: path, optimized
-			kCmdExportPosesGlobal,
-			kCmdExportPosesLocal,
+			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
+			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=KITTI/raw format, 1=RGBD-SLAM format, 2=TORO)
 			kCmdCleanDataBuffer,
-			kCmdPublish3DMapLocal, // params: optimized
-			kCmdPublish3DMapGlobal, // params: optimized
-			kCmdPublishTOROGraphGlobal, // params: optimized
-			kCmdPublishTOROGraphLocal, // params: optimized
+			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
 			kCmdTriggerNewMap,
 			kCmdPause,
-			kCmdGoal, // params: label or location ID
+			kCmdGoal,             // params: [string] label or [int] location ID
 			kCmdCancelGoal,
-			kCmdLabel}; // // params: label or location ID
+			kCmdLabel             // params: [string] label or [int] location ID
+	};
 public:
-	RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) :
+	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :
 			UEvent(0),
-			_cmd(cmd),
-			_strValue(strValue),
-			_intValue(intValue),
-			_parameters(parameters){}
+			cmd_(cmd),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			value3_(value3),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const UVariant & value4, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			value3_(value3),
+			value4_(value4),
+			parameters_(parameters){}
 
 	virtual ~RtabmapEventCmd() {}
-	Cmd getCmd() const {return _cmd;}
+	Cmd getCmd() const {return cmd_;}
 
-	void setStr(const std::string & str) {_strValue = str;}
-	const std::string & getStr() const {return _strValue;}
+	const UVariant & value1() const {return value1_;}
+	const UVariant & value2() const {return value2_;}
+	const UVariant & value3() const {return value3_;}
+	const UVariant & value4() const {return value4_;}
 
-	void setInt(int v) {_intValue = v;}
-	int getInt() const {return _intValue;}
-
-	const ParametersMap & getParameters() const {return _parameters;}
+	const ParametersMap & getParameters() const {return parameters_;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
 
 private:
-	Cmd _cmd;
-	std::string _strValue;
-	int _intValue;
-	ParametersMap _parameters;
+	Cmd cmd_;
+	UVariant value1_;
+	UVariant value2_;
+	UVariant value3_;
+	UVariant value4_;
+	ParametersMap parameters_;
 };
 
 class RtabmapEventInit : public UEvent

corelib/include/rtabmap/core/RtabmapThread.h

@@ -61,17 +61,10 @@ public:
 		kStateChangingParameters,
 		kStateDumpingMemory,
 		kStateDumpingPrediction,
-		kStateGeneratingDOTGraph,
-		kStateGeneratingDOTLocalGraph,
-		kStateGeneratingTOROGraphLocal,
-		kStateGeneratingTOROGraphGlobal,
-		kStateExportingPosesLocal,
-		kStateExportingPosesGlobal,
+		kStateExportingDOTGraph,
+		kStateExportingPoses,
 		kStateCleanDataBuffer,
-		kStatePublishingMapLocal,
-		kStatePublishingMapGlobal,
-		kStatePublishingTOROGraphLocal,
-		kStatePublishingTOROGraphGlobal,
+		kStatePublishingMap,
 		kStateTriggeringMap,
 		kStateAddingUserData,
 		kStateSettingGoal,
@@ -99,8 +92,7 @@ private:
 	void addData(const OdometryEvent & odomEvent);
 	bool getData(OdometryEvent & data);
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
-	void publishMap(bool optimized, bool full) const;
-	void publishGraph(bool optimized, bool full) const;
+	void publishMap(bool optimized, bool full, bool graphOnly) const;
 
 private:
 	UMutex _stateMutex;

corelib/src/Rtabmap.cpp

@@ -718,7 +718,7 @@ void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
 	}
 }
 
-void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool global)
+void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int type)
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -735,40 +735,70 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
-		graph::TOROOptimizer::saveGraph(path, poses, constraints);
-	}
-}
-
-void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global)
-{
-	if(_memory && _memory->getLastWorkingSignature())
-	{
-		std::map<int, Transform> poses;
-		std::multimap<int, Link> constraints;
-
-		if(optimized)
+		if(type==2) // TORO
 		{
-			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+			graph::TOROOptimizer::saveGraph(path, poses, constraints);
 		}
 		else
 		{
-			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
-			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
-		}
+			//get timestamps
+			std::map<int, double> stamps;
+			if(type == 1)
+			{
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					Transform o;
+					int m, w;
+					std::string l;
+					double stamp = 0.0;
+					_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
+					stamps.insert(std::make_pair(iter->first, stamp));
+				}
+				UASSERT(stamps.size()== 0 || stamps.size() == poses.size());
+			}
 
-		//get timestamps
-		std::map<int, double> stamps;
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-		{
-			Transform o;
-			int m, w;
-			std::string l;
-			double stamp = 0.0;
-			_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
-			stamps.insert(std::make_pair(iter->first, stamp));
-		}
+			FILE* fout = 0;
+#ifdef _MSC_VER
+			fopen_s(&fout, path.c_str(), "w");
+#else
+			fout = fopen(path.c_str(), "w");
+#endif
+			if(fout)
+			{
+				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					if(type == 1) // rgbd-slam format
+					{
+						// Format: stamp x y z qw qx qy qz
+						Eigen::Quaternionf q = (*iter).second.getQuaternionf();
 
-		this->dumpPoses(path, poses, stamps);
+						UASSERT(uContains(stamps, iter->first));
+						fprintf(fout, "%f %f %f %f %f %f %f %f\n",
+								stamps.at(iter->first),
+								(*iter).second.x(),
+								(*iter).second.y(),
+								(*iter).second.z(),
+								q.w(),
+								q.x(),
+								q.y(),
+								q.z());
+					}
+					else // default / KITTI format
+					{
+						// Format: r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+						const float * p = (const float *)(*iter).second.data();
+
+						fprintf(fout, "%f", p[0]);
+						for(int i=1; i<(*iter).second.size(); i++)
+						{
+							fprintf(fout, " %f", p[i]);
+						}
+						fprintf(fout, "\n");
+					}
+				}
+				fclose(fout);
+			}
+		}
 	}
 }
 
@@ -2476,46 +2506,6 @@ void Rtabmap::dumpData() const
 	}
 }
 
-void Rtabmap::dumpPoses(
-		const std::string & path,
-		const std::map<int, Transform> & poses,
-		const std::map<int, double> & stamps) const
-{
-	UDEBUG("");
-	UASSERT(stamps.size()== 0 || stamps.size() == poses.size());
-	FILE* fout = 0;
-#ifdef _MSC_VER
-	fopen_s(&fout, path.c_str(), "w");
-#else
-	fout = fopen(path.c_str(), "w");
-#endif
-	if(fout)
-	{
-		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-		{
-			// in camera frame
-			const float * p = (const float *)(*iter).second.data();
-
-			int index = 0;
-			if(stamps.size() == poses.size())
-			{
-				UASSERT(uContains(stamps, iter->first));
-				fprintf(fout, "%f", stamps.at(iter->first));
-			}
-			else
-			{
-				fprintf(fout, "%f", p[index++]);
-			}
-			for(int i=index; i<(*iter).second.size(); i++)
-			{
-				fprintf(fout, " %f", p[i]);
-			}
-			fprintf(fout, "\n");
-		}
-		fclose(fout);
-	}
-}
-
 // fromId must be in _memory and in _optimizedPoses
 // Get poses in front of the robot, return optimized poses
 std::map<int, Transform> Rtabmap::getForwardWMPoses(

corelib/src/RtabmapThread.cpp

@@ -117,7 +117,7 @@ void RtabmapThread::createIntermediateNodes(bool enabled)
 	enabled = _createIntermediateNodes;
 }
 
-void RtabmapThread::publishMap(bool optimized, bool full) const
+void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
 {
 	std::map<int, Signature> signatures;
 	std::map<int, Transform> poses;
@@ -127,11 +127,23 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	std::map<int, std::string> labels;
 	std::map<int, std::vector<unsigned char> > userDatas;
 
-	_rtabmap->get3DMap(signatures,
-			poses,
-			constraints,
-			optimized,
-			full);
+	if(graphOnly)
+	{
+		_rtabmap->getGraph(poses,
+				constraints,
+				optimized,
+				full,
+				&signatures);
+	}
+	else
+	{
+		_rtabmap->get3DMap(
+				signatures,
+				poses,
+				constraints,
+				optimized,
+				full);
+	}
 
 	this->post(new RtabmapEvent3DMap(
 			signatures,
@@ -139,29 +151,6 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 			constraints));
 }
 
-void RtabmapThread::publishGraph(bool optimized, bool full) const
-{
-	std::map<int, Signature> signatures;
-	std::map<int, Transform> poses;
-	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
-	std::map<int, double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-
-	_rtabmap->getGraph(poses,
-			constraints,
-			optimized,
-			full,
-			&signatures);
-
-	this->post(new RtabmapEvent3DMap(
-			signatures,
-			poses,
-			constraints));
-}
-
-
 void RtabmapThread::mainLoopKill()
 {
 	this->clearBufferedData();
@@ -229,38 +218,27 @@ void RtabmapThread::mainLoop()
 	case kStateDumpingPrediction:
 		_rtabmap->dumpPrediction();
 		break;
-	case kStateGeneratingDOTGraph:
-		_rtabmap->generateDOTGraph(parameters.at("path"));
+	case kStateExportingDOTGraph:
+		_rtabmap->generateDOTGraph(
+				parameters.at("path"),
+				atoi(parameters.at("id").c_str()),
+				atoi(parameters.at("margin").c_str()));
 		break;
-	case kStateGeneratingDOTLocalGraph:
-		_rtabmap->generateDOTGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
-		break;
-	case kStateGeneratingTOROGraphLocal:
-		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStateGeneratingTOROGraphGlobal:
-		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
-		break;
-	case kStateExportingPosesLocal:
-		_rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStateExportingPosesGlobal:
-		_rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
+	case kStateExportingPoses:
+		_rtabmap->exportPoses(
+				parameters.at("path"),
+				uStr2Bool(parameters.at("optimized")),
+				uStr2Bool(parameters.at("global")),
+				atoi(parameters.at("type").c_str()));
 		break;
 	case kStateCleanDataBuffer:
 		this->clearBufferedData();
 		break;
-	case kStatePublishingMapLocal:
-		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStatePublishingMapGlobal:
-		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
-		break;
-	case kStatePublishingTOROGraphLocal:
-		this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStatePublishingTOROGraphGlobal:
-		this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, true);
+	case kStatePublishingMap:
+		this->publishMap(
+				uStr2Bool(parameters.at("optimized")),
+				uStr2Bool(parameters.at("global")),
+				uStr2Bool(parameters.at("graph_only")));
 		break;
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();
@@ -275,10 +253,10 @@ void RtabmapThread::mainLoop()
 		_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 = atoi(parameters.at("id").c_str());
+		if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
 		{
-			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("goal_label"));
+			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
 		}
 		if(id <= 0 || !_rtabmap->computePath(id, true))
 		{
@@ -360,8 +338,8 @@ void RtabmapThread::handleEvent(UEvent* event)
 		{
 			ULOGGER_DEBUG("CMD_INIT");
 			ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
-			UASSERT(!rtabmapEvent->getStr().empty());
-			UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second);
+			UASSERT(rtabmapEvent->value1().isStr());
+			UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
 			pushNewState(kStateInit, parameters);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdClose)
@@ -386,68 +364,30 @@ void RtabmapThread::handleEvent(UEvent* event)
 		}
 		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
 		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
 			ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isStr());
+			UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
+			UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			pushNewState(kStateGeneratingDOTGraph, param);
+			param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
+			param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
+			param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
+			pushNewState(kStateExportingDOTGraph, param);
 		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTLocalGraph)
+		else if(cmd == RtabmapEventCmd::kCmdExportPoses)
 		{
-			std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
-			UASSERT(values.size() == 3);
-
-			ULOGGER_DEBUG("CMD_GENERATE_DOT_LOCAL_GRAPH");
+			ULOGGER_DEBUG("CMD_EXPORT_POSES");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isBool());
+			UASSERT(rtabmapEvent->value3().isStr());
+			UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("path", *values.begin()));
-			param.insert(ParametersPair("id", *(++values.begin())));
-			param.insert(ParametersPair("margin", *values.rbegin()));
-			pushNewState(kStateGeneratingDOTLocalGraph, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphLocal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_LOCAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateGeneratingTOROGraphLocal, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphGlobal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateGeneratingTOROGraphGlobal, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdExportPosesLocal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_EXPORT_POSES_LOCAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateExportingPosesLocal, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdExportPosesGlobal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_EXPORT_POSES_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateExportingPosesGlobal, param);
+			param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
+			param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
+			pushNewState(kStateExportingPoses, param);
 
 		}
 		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
@@ -455,33 +395,17 @@ void RtabmapThread::handleEvent(UEvent* event)
 			ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
 			pushNewState(kStateCleanDataBuffer);
 		}
-		else if(cmd == RtabmapEventCmd::kCmdPublish3DMapLocal)
+		else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
 		{
-			ULOGGER_DEBUG("CMD_PUBLISH_MAP_LOCAL");
+			ULOGGER_DEBUG("CMD_PUBLISH_MAP");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isBool());
+			UASSERT(rtabmapEvent->value3().isBool());
 			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingMapLocal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublish3DMapGlobal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_MAP_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingMapGlobal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphLocal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_LOCAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingTOROGraphLocal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingTOROGraphGlobal, param);
+			param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
+			param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
+			pushNewState(kStatePublishingMap, param);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
 		{
@@ -496,9 +420,10 @@ void RtabmapThread::handleEvent(UEvent* event)
 		else if(cmd == RtabmapEventCmd::kCmdGoal)
 		{
 			ULOGGER_DEBUG("CMD_GOAL");
+			UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("goal_label", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
+			param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
+			param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
 			pushNewState(kStateSettingGoal, param);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
@@ -509,9 +434,10 @@ void RtabmapThread::handleEvent(UEvent* event)
 		else if(cmd == RtabmapEventCmd::kCmdLabel)
 		{
 			ULOGGER_DEBUG("CMD_LABEL");
+			UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("label", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("id", uNumber2Str(rtabmapEvent->getInt())));
+			param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
+			param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
 			pushNewState(kStateLabelling, param);
 		}
 		else