diff --git a/rtabmap/msg/RtabmapInfo.msg b/rtabmap/msg/RtabmapInfo.msg
index 9a386aa5..40da6aed 100644
--- a/rtabmap/msg/RtabmapInfo.msg
+++ b/rtabmap/msg/RtabmapInfo.msg
@@ -1,8 +1,10 @@
-#
-#
-#
-#
-#
+
+########################################
+# If a loop is found with the current image ("refId"), 
+# "loopClosureId" is not null. Field "actuators" contains 
+# (when actions are used) actions executed after the 
+# "loopClosureId".
+########################################
 
 Header header
 
@@ -10,4 +12,6 @@ int32 refId
 int32 loopClosureId
 
 uint32 actuatorStep
-float32[] actuators
\ No newline at end of file
+float32[] actuators
+
+rtabmap/RtabmapInfoEx infoEx
\ No newline at end of file
diff --git a/rtabmap/msg/RtabmapInfoEx.msg b/rtabmap/msg/RtabmapInfoEx.msg
index 2777e665..9dbef419 100644
--- a/rtabmap/msg/RtabmapInfoEx.msg
+++ b/rtabmap/msg/RtabmapInfoEx.msg
@@ -1,13 +1,12 @@
-#
-#
-#
-#
-#
+
+########################################
+# Statistics stuff: 
+# These fields are empty if RTAb-Map's 
+# parameter publishStats=false
+########################################
 
 Header header
 
-rtabmap/RtabmapInfo info
-
 sensor_msgs/CompressedImage refImage
 int32 refChild
 
@@ -40,4 +39,5 @@ rtabmap/KeyPoint[] loopWordsValues
 
 ##SM masks##
 uint8[] refMotionMask
-uint8[] loopMotionMask
\ No newline at end of file
+uint8[] loopMotionMask
+
diff --git a/rtabmap/src/CameraWrapper.cpp b/rtabmap/src/CameraWrapper.cpp
index a3e36dbc..d8d89f94 100644
--- a/rtabmap/src/CameraWrapper.cpp
+++ b/rtabmap/src/CameraWrapper.cpp
@@ -76,9 +76,7 @@ bool CameraWrapper::changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Re
 {
 	ros::NodeHandle nh("~");
 	nh.setParam("image_hz", request.imgRate);
-	nh.setParam("auto_restart", request.autoRestart);
 	camera_->setImageRate(request.imgRate);
-	camera_->setAutoRestart(request.autoRestart);
 	return true;
 }
 
diff --git a/rtabmap/src/CoreWrapper.cpp b/rtabmap/src/CoreWrapper.cpp
index 5360196f..8a013859 100644
--- a/rtabmap/src/CoreWrapper.cpp
+++ b/rtabmap/src/CoreWrapper.cpp
@@ -26,7 +26,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 {
 	ros::NodeHandle nh("~");
 	infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1);
-	infoExPub_ = nh.advertise<rtabmap::RtabmapInfoEx>("info_x", 1);
 	parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
 
 	rtabmap_ = new Rtabmap();
@@ -46,9 +45,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	nh = ros::NodeHandle();
 	smStateTopic_ = nh.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this);
-	imageTopic_ = nh.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
 	parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
 
+	image_transport::ImageTransport it(nh);
+	imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
+
 	UEventsManager::addHandler(this);
 }
 
@@ -200,8 +201,8 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
 
 		if(stat.extended())
 		{
-			rtabmap::RtabmapInfoExPtr msg(new rtabmap::RtabmapInfoEx);
-			msg->info.refId = stat.refImageId();
+			rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
+			msg->refId = stat.refImageId();
 			if(stat.refImage())
 			{
 				int params[3] = {0};
@@ -229,9 +230,9 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
 				memcpy(&compressed.data[0], buf->data.ptr, buf->width);
 				cvReleaseMat(&buf);
 
-				msg->refImage = compressed;
+				msg->infoEx.refImage = compressed;
 			}
-			msg->info.loopClosureId = stat.loopClosureId();
+			msg->loopClosureId = stat.loopClosureId();
 			if(stat.loopClosureImage())
 			{
 				int params[3] = {0};
@@ -259,81 +260,9 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
 				memcpy(&compressed.data[0], buf->data.ptr, buf->width);
 				cvReleaseMat(&buf);
 
-				msg->loopClosureImage = compressed;
+				msg->infoEx.loopClosureImage = compressed;
 			}
 
-			const std::list<std::vector<float> > & actuators = stat.getActions();
-			if(actuators.size())
-			{
-				msg->info.actuatorStep = actuators.front().size();
-			}
-			for(std::list<std::vector<float> >::const_iterator iter=actuators.begin();iter!=actuators.end();++iter)
-			{
-				if((iter->size() == 0 && msg->info.actuatorStep > 0) || msg->info.actuatorStep % iter->size() != 0)
-				{
-					ROS_ERROR("Actuators must have all the same length.");
-				}
-				msg->info.actuators.insert(msg->info.actuators.end(), iter->begin(), iter->end());
-			}
-
-			//Posterior, likelihood, childCount
-			msg->posteriorKeys = uKeys(stat.posterior());
-			msg->posteriorValues = uValues(stat.posterior());
-			msg->likelihoodKeys = uKeys(stat.likelihood());
-			msg->likelihoodValues = uValues(stat.likelihood());
-			msg->weightsKeys = uKeys(stat.weights());
-			msg->weightsValues = uValues(stat.weights());
-
-			//SURF stuff...
-			msg->refWordsKeys = uListToVector(uKeys(stat.refWords()));
-			msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
-			int index = 0;
-			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
-				i!=stat.refWords().end();
-				++i)
-			{
-				msg->refWordsValues.at(index).angle = i->second.angle;
-				msg->refWordsValues.at(index).response = i->second.response;
-				msg->refWordsValues.at(index).ptx = i->second.pt.x;
-				msg->refWordsValues.at(index).pty = i->second.pt.y;
-				msg->refWordsValues.at(index).size = i->second.size;
-				msg->refWordsValues.at(index).octave = i->second.octave;
-				msg->refWordsValues.at(index).class_id = i->second.class_id;
-				++index;
-			}
-			msg->loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
-			msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
-			index = 0;
-			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
-				i!=stat.loopWords().end();
-				++i)
-			{
-				msg->loopWordsValues.at(index).angle = i->second.angle;
-				msg->loopWordsValues.at(index).response = i->second.response;
-				msg->loopWordsValues.at(index).ptx = i->second.pt.x;
-				msg->loopWordsValues.at(index).pty = i->second.pt.y;
-				msg->loopWordsValues.at(index).size = i->second.size;
-				msg->loopWordsValues.at(index).octave = i->second.octave;
-				msg->loopWordsValues.at(index).class_id = i->second.class_id;
-				++index;
-			}
-
-			// SM masks
-			msg->refMotionMask = stat.refMotionMask();
-			msg->loopMotionMask = stat.loopMotionMask();
-
-			// Statistics data
-			msg->statsKeys = uKeys(stat.data());
-			msg->statsValues = uValues(stat.data());
-
-			infoExPub_.publish(msg);
-		}
-		else
-		{
-			rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
-			ROS_INFO("Loop closure detected! newId=%d with oldId=%d", stat.refImageId(), stat.loopClosureId());
-			msg->refId = stat.refImageId();
-			msg->loopClosureId = stat.loopClosureId();
 			const std::list<std::vector<float> > & actuators = stat.getActions();
 			if(actuators.size())
 			{
@@ -347,6 +276,57 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
 				}
 				msg->actuators.insert(msg->actuators.end(), iter->begin(), iter->end());
 			}
+
+			//Posterior, likelihood, childCount
+			msg->infoEx.posteriorKeys = uKeys(stat.posterior());
+			msg->infoEx.posteriorValues = uValues(stat.posterior());
+			msg->infoEx.likelihoodKeys = uKeys(stat.likelihood());
+			msg->infoEx.likelihoodValues = uValues(stat.likelihood());
+			msg->infoEx.weightsKeys = uKeys(stat.weights());
+			msg->infoEx.weightsValues = uValues(stat.weights());
+
+			//SURF stuff...
+			msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords()));
+			msg->infoEx.refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
+			int index = 0;
+			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
+				i!=stat.refWords().end();
+				++i)
+			{
+				msg->infoEx.refWordsValues.at(index).angle = i->second.angle;
+				msg->infoEx.refWordsValues.at(index).response = i->second.response;
+				msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x;
+				msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y;
+				msg->infoEx.refWordsValues.at(index).size = i->second.size;
+				msg->infoEx.refWordsValues.at(index).octave = i->second.octave;
+				msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id;
+				++index;
+			}
+			msg->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
+			msg->infoEx.loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
+			index = 0;
+			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
+				i!=stat.loopWords().end();
+				++i)
+			{
+				msg->infoEx.loopWordsValues.at(index).angle = i->second.angle;
+				msg->infoEx.loopWordsValues.at(index).response = i->second.response;
+				msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x;
+				msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y;
+				msg->infoEx.loopWordsValues.at(index).size = i->second.size;
+				msg->infoEx.loopWordsValues.at(index).octave = i->second.octave;
+				msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id;
+				++index;
+			}
+
+			// SM masks
+			msg->infoEx.refMotionMask = stat.refMotionMask();
+			msg->infoEx.loopMotionMask = stat.loopMotionMask();
+
+			// Statistics data
+			msg->infoEx.statsKeys = uKeys(stat.data());
+			msg->infoEx.statsValues = uValues(stat.data());
+
 			infoPub_.publish(msg);
 		}
 	}
diff --git a/rtabmap/src/CoreWrapper.h b/rtabmap/src/CoreWrapper.h
index 7c8fde46..69cef365 100644
--- a/rtabmap/src/CoreWrapper.h
+++ b/rtabmap/src/CoreWrapper.h
@@ -17,6 +17,7 @@
 #include <rtabmap/core/RtabmapEvent.h>
 #include <sensor_msgs/Image.h>
 #include "rtabmap/SensoryMotorState.h"
+#include <image_transport/image_transport.h>
 
 namespace rtabmap
 {
@@ -49,10 +50,9 @@ private:
 private:
 	rtabmap::Rtabmap * rtabmap_;
 	ros::Subscriber smStateTopic_;
-	ros::Subscriber imageTopic_;
+	image_transport::Subscriber imageTopic_;
 	ros::Subscriber parametersUpdatedTopic_;
 	ros::Publisher infoPub_;
-	ros::Publisher infoExPub_;
 	ros::Publisher parametersLoadedPub_;
 	std::string configFile_;
 
diff --git a/rtabmap/src/GuiWrapper.cpp b/rtabmap/src/GuiWrapper.cpp
index d7c1ce65..920d66fd 100644
--- a/rtabmap/src/GuiWrapper.cpp
+++ b/rtabmap/src/GuiWrapper.cpp
@@ -27,7 +27,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 {
 	ros::NodeHandle nh;
 	infoTopic_ = nh.subscribe("rtabmap/info", 1, &GuiWrapper::infoReceivedCallback, this);
-	infoExTopic_ = nh.subscribe("rtabmap/info_x", 1, &GuiWrapper::infoExReceivedCallback, this);
 	velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
 	app_ = new QApplication(argc, argv);
 	mainWindow_ = new MainWindow(new PreferencesDialogROS());
@@ -63,10 +62,83 @@ int GuiWrapper::exec()
 
 void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 {
-	ROS_INFO("Loop closure detected! newId=%d with oldId=%d", msg->refId, msg->loopClosureId);
+	ROS_INFO("RTAB-Map info received!");
+
+	// Map from ROS struct to rtabmap struct
 	rtabmap::Statistics * stat = new rtabmap::Statistics();
+
+	stat->setExtended(true); // Extended
+
 	stat->setRefImageId(msg->refId);
+	if(msg->infoEx.refImage.data.size() > 0)
+	{
+		// Decompress
+		const CvMat compressed = cvMat(1, msg->infoEx.refImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->infoEx.refImage.data[0]));
+		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
+		stat->setRefImage(&decompressed);
+	}
 	stat->setLoopClosureId(msg->loopClosureId);
+	if(msg->infoEx.loopClosureImage.data.size() > 0)
+	{
+		// Decompress
+		const CvMat compressed = cvMat(1, msg->infoEx.loopClosureImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->infoEx.loopClosureImage.data[0]));
+		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
+		stat->setLoopClosureImage(&decompressed);
+	}
+
+	//Posterior, likelihood, childCount
+	std::map<int, float> mapIntFloat;
+	for(unsigned int i=0; i<msg->infoEx.posteriorKeys.size() && i<msg->infoEx.posteriorValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->infoEx.posteriorKeys.at(i), msg->infoEx.posteriorValues.at(i)));
+	}
+	stat->setPosterior(mapIntFloat);
+	mapIntFloat.clear();
+	for(unsigned int i=0; i<msg->infoEx.likelihoodKeys.size() && i<msg->infoEx.likelihoodValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->infoEx.likelihoodKeys.at(i), msg->infoEx.likelihoodValues.at(i)));
+	}
+	stat->setLikelihood(mapIntFloat);
+	std::map<int, int> mapIntInt;
+	for(unsigned int i=0; i<msg->infoEx.weightsKeys.size() && i<msg->infoEx.weightsValues.size(); ++i)
+	{
+		mapIntInt.insert(std::pair<int, int>(msg->infoEx.weightsKeys.at(i), msg->infoEx.weightsValues.at(i)));
+	}
+	stat->setWeights(mapIntInt);
+
+	//SURF stuff...
+	std::multimap<int, cv::KeyPoint> mapIntKeypoint;
+	for(unsigned int i=0; i<msg->infoEx.refWordsKeys.size() && i<msg->infoEx.refWordsValues.size(); i++)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->infoEx.refWordsValues.at(i).angle;
+		pt.response = msg->infoEx.refWordsValues.at(i).response;
+		//pt.laplacian = msg->refWordsValues.at(i).laplacian;
+		pt.pt.x = msg->infoEx.refWordsValues.at(i).ptx;
+		pt.pt.y = msg->infoEx.refWordsValues.at(i).pty;
+		pt.size = msg->infoEx.refWordsValues.at(i).size;
+		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->infoEx.refWordsKeys.at(i), pt));
+	}
+	stat->setRefWords(mapIntKeypoint);
+	mapIntKeypoint.clear();
+	for(unsigned int i=0; i<msg->infoEx.loopWordsKeys.size() && i<msg->infoEx.loopWordsValues.size(); i++)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->infoEx.loopWordsValues.at(i).angle;
+		pt.response = msg->infoEx.loopWordsValues.at(i).response;
+		//pt.laplacian = msg->loopWordsValues.at(i).laplacian;
+		pt.pt.x = msg->infoEx.loopWordsValues.at(i).ptx;
+		pt.pt.y = msg->infoEx.loopWordsValues.at(i).pty;
+		pt.size = msg->infoEx.loopWordsValues.at(i).size;
+		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->infoEx.loopWordsKeys.at(i), pt));
+	}
+	stat->setLoopWords(mapIntKeypoint);
+
+	//SM stuff
+	stat->setRefMotionMask(msg->infoEx.refMotionMask);
+	stat->setLoopMotionMask(msg->infoEx.loopMotionMask);
+
+	//Actions
 	std::list<std::vector<float> > actions;
 	for(unsigned int i=0; i<msg->actuators.size(); i+=msg->actuatorStep)
 	{
@@ -78,104 +150,11 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 		actions.push_back(a);
 	}
 	stat->setActions(actions);
-	UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
-}
-
-void GuiWrapper::infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
-{
-	ROS_INFO("Statistics received!");
-
-	// Map from ROS struct to rtabmap struct
-	rtabmap::Statistics * stat = new rtabmap::Statistics();
-
-	stat->setExtended(true); // Extended
-
-	stat->setRefImageId(msg->info.refId);
-	if(msg->refImage.data.size() > 0)
-	{
-		// Decompress
-		const CvMat compressed = cvMat(1, msg->refImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->refImage.data[0]));
-		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
-		stat->setRefImage(&decompressed);
-	}
-	stat->setLoopClosureId(msg->info.loopClosureId);
-	if(msg->loopClosureImage.data.size() > 0)
-	{
-		// Decompress
-		const CvMat compressed = cvMat(1, msg->loopClosureImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->loopClosureImage.data[0]));
-		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
-		stat->setLoopClosureImage(&decompressed);
-	}
-
-	//Posterior, likelihood, childCount
-	std::map<int, float> mapIntFloat;
-	for(unsigned int i=0; i<msg->posteriorKeys.size() && i<msg->posteriorValues.size(); ++i)
-	{
-		mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
-	}
-	stat->setPosterior(mapIntFloat);
-	mapIntFloat.clear();
-	for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
-	{
-		mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
-	}
-	stat->setLikelihood(mapIntFloat);
-	std::map<int, int> mapIntInt;
-	for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
-	{
-		mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
-	}
-	stat->setWeights(mapIntInt);
-
-	//SURF stuff...
-	std::multimap<int, cv::KeyPoint> mapIntKeypoint;
-	for(unsigned int i=0; i<msg->refWordsKeys.size() && i<msg->refWordsValues.size(); i++)
-	{
-		cv::KeyPoint pt;
-		pt.angle = msg->refWordsValues.at(i).angle;
-		pt.response = msg->refWordsValues.at(i).response;
-		//pt.laplacian = msg->refWordsValues.at(i).laplacian;
-		pt.pt.x = msg->refWordsValues.at(i).ptx;
-		pt.pt.y = msg->refWordsValues.at(i).pty;
-		pt.size = msg->refWordsValues.at(i).size;
-		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->refWordsKeys.at(i), pt));
-	}
-	stat->setRefWords(mapIntKeypoint);
-	mapIntKeypoint.clear();
-	for(unsigned int i=0; i<msg->loopWordsKeys.size() && i<msg->loopWordsValues.size(); i++)
-	{
-		cv::KeyPoint pt;
-		pt.angle = msg->loopWordsValues.at(i).angle;
-		pt.response = msg->loopWordsValues.at(i).response;
-		//pt.laplacian = msg->loopWordsValues.at(i).laplacian;
-		pt.pt.x = msg->loopWordsValues.at(i).ptx;
-		pt.pt.y = msg->loopWordsValues.at(i).pty;
-		pt.size = msg->loopWordsValues.at(i).size;
-		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->loopWordsKeys.at(i), pt));
-	}
-	stat->setLoopWords(mapIntKeypoint);
-
-	//SM stuff
-	stat->setRefMotionMask(msg->refMotionMask);
-	stat->setLoopMotionMask(msg->loopMotionMask);
-
-	//Actions
-	std::list<std::vector<float> > actions;
-	for(unsigned int i=0; i<msg->info.actuators.size(); i+=msg->info.actuatorStep)
-	{
-		std::vector<float> a(msg->info.actuatorStep);
-		for(unsigned int j=0; j<a.size(); ++j)
-		{
-			a[j] = msg->info.actuators[i+j];
-		}
-		actions.push_back(a);
-	}
-	stat->setActions(actions);
 
 	// Statistics data
-	for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
+	for(unsigned int i=0; i<msg->infoEx.statsKeys.size() && i<msg->infoEx.statsValues.size(); i++)
 	{
-		stat->addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
+		stat->addStatistic(msg->infoEx.statsKeys.at(i), msg->infoEx.statsValues.at(i));
 	}
 
 	ROS_INFO("Publishing statistics...");
diff --git a/rtabmap/src/GuiWrapper.h b/rtabmap/src/GuiWrapper.h
index 2b9cb7f0..4d850c71 100644
--- a/rtabmap/src/GuiWrapper.h
+++ b/rtabmap/src/GuiWrapper.h
@@ -34,12 +34,10 @@ protected:
 
 private:
 	void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
-	void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & infoExMsg);
 	void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg);
 
 private:
 	ros::Subscriber infoTopic_;
-	ros::Subscriber infoExTopic_;
 	ros::Subscriber velocity_sub_;
 	QApplication * app_;
 	rtabmap::MainWindow * mainWindow_;
diff --git a/rtabmap/src/OutputNode.cpp b/rtabmap/src/OutputNode.cpp
index 5c2c937b..00a86b06 100644
--- a/rtabmap/src/OutputNode.cpp
+++ b/rtabmap/src/OutputNode.cpp
@@ -42,11 +42,6 @@ void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 	publishCommands(msg->actuators, msg->actuatorStep);
 }
 
-void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
-{
-	publishCommands(msg->info.actuators, msg->info.actuatorStep);
-}
-
 int main(int argc, char** argv)
 {
 	ros::init(argc, argv, "rtabmap_out");
@@ -70,7 +65,6 @@ int main(int argc, char** argv)
 	ros::Subscriber infoTopic;
 	ros::Subscriber infoExTopic;
 	infoTopic = nh.subscribe("rtabmap/info", 1, infoReceivedCallback);
-	infoExTopic = nh.subscribe("rtabmap/info_x", 1, infoExReceivedCallback);
 	rosPublisher = nh.advertise<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
 
 	ros::spin();
diff --git a/rtabmap/srv/ChangeCameraImgRate.srv b/rtabmap/srv/ChangeCameraImgRate.srv
index 50cde067..84920a78 100644
--- a/rtabmap/srv/ChangeCameraImgRate.srv
+++ b/rtabmap/srv/ChangeCameraImgRate.srv
@@ -1,3 +1,2 @@
 float32 imgRate
-bool autoRestart
 ---
\ No newline at end of file