Merged pcl_integration branch to trunk

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

rtabmap/src/CameraNode.cpp

@@ -0,0 +1,285 @@
+/*
+ * CameraNode.cpp
+ *
+ *  Created on: 1 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <std_msgs/Empty.h>
+#include <image_transport/image_transport.h>
+#include <std_srvs/Empty.h>
+
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/CameraThread.h>
+#include <rtabmap/core/CameraEvent.h>
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <rtabmap/CameraConfig.h>
+
+class CameraWrapper : public UEventsHandler
+{
+public:
+	// Usb device like a Webcam
+	CameraWrapper(int usbDevice = 0,
+			      float imageRate = 0,
+				  unsigned int imageWidth = 0,
+				  unsigned int imageHeight = 0) :
+		camera_(0),
+		frameId_("camera")
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle pnh("~");
+		pnh.param("frame_id", frameId_, frameId_);
+		image_transport::ImageTransport it(nh);
+		rosPublisher_ = it.advertise("image", 1);
+		startSrv_ = nh.advertiseService("start_camera", &CameraWrapper::startSrv, this);
+		stopSrv_ = nh.advertiseService("stop_camera", &CameraWrapper::stopSrv, this);
+		UEventsManager::addHandler(this);
+	}
+
+	virtual ~CameraWrapper()
+	{
+		if(camera_)
+		{
+			camera_->join(true);
+			delete camera_;
+		}
+	}
+
+	bool init()
+	{
+		if(camera_)
+		{
+			return camera_->init();
+		}
+		return false;
+	}
+
+	void start()
+	{
+		if(camera_)
+		{
+			return camera_->start();
+		}
+	}
+
+	bool startSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("Camera started...");
+		if(camera_)
+		{
+			camera_->start();
+		}
+		return true;
+	}
+
+	bool stopSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("Camera stopped...");
+		if(camera_)
+		{
+			camera_->kill();
+		}
+		return true;
+	}
+
+	void setParameters(int deviceId, double frameRate, int width, int height, const std::string & path, bool autoRestart, bool pause)
+	{
+		ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f w/h=%d/%d autoRestart=%s pause=%s",
+				deviceId, path.c_str(), frameRate, width, height, autoRestart?"true":"false", pause?"true":"false");
+		if(camera_)
+		{
+			rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_->getCamera());
+			rtabmap::CameraImages * imagesCam = dynamic_cast<rtabmap::CameraImages *>(camera_->getCamera());
+
+			if(imagesCam)
+			{
+				// images
+				if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0)
+				{
+					imagesCam->setImageRate(frameRate);
+					imagesCam->setImageSize(width, height);
+					camera_->setAutoRestart(autoRestart);
+					if(pause && !camera_->isPaused())
+					{
+						camera_->join(true);
+					}
+					else if(!pause && camera_->isPaused())
+					{
+						camera_->start();
+					}
+				}
+				else
+				{
+					delete camera_;
+					camera_ = 0;
+				}
+			}
+			else if(videoCam)
+			{
+				if(!path.empty() && path.compare(videoCam->getFilePath()) == 0)
+				{
+					// video
+					videoCam->setImageRate(frameRate);
+					videoCam->setImageSize(width, height);
+					camera_->setAutoRestart(autoRestart);
+					if(pause && !camera_->isPaused())
+					{
+						camera_->join(true);
+					}
+					else if(!pause && camera_->isPaused())
+					{
+						camera_->start();
+					}
+				}
+				else if(path.empty() &&
+						videoCam->getFilePath().empty() &&
+						videoCam->getUsbDevice() == deviceId)
+				{
+					// usb device
+					unsigned int w;
+					unsigned int h;
+					videoCam->getImageSize(w, h);
+					if((int)w == width && (int)h == height)
+					{
+						videoCam->setImageRate(frameRate);
+						camera_->setAutoRestart(autoRestart);
+						if(pause && !camera_->isPaused())
+						{
+							camera_->join(true);
+						}
+						else if(!pause && camera_->isPaused())
+						{
+							camera_->start();
+						}
+					}
+					else
+					{
+						delete camera_;
+						camera_ = 0;
+					}
+				}
+				else
+				{
+					delete camera_;
+					camera_ = 0;
+				}
+			}
+			else
+			{
+				ROS_ERROR("Wrong camera type ?!?");
+				delete camera_;
+				camera_ = 0;
+			}
+		}
+
+		if(!camera_)
+		{
+			if(!path.empty() && UDirectory::exists(path))
+			{
+				//images
+				camera_ = new rtabmap::CameraThread(new rtabmap::CameraImages(path, 1, false, frameRate, width, height), autoRestart);
+			}
+			else if(!path.empty() && UFile::exists(path))
+			{
+				//video
+				camera_ = new rtabmap::CameraThread(new rtabmap::CameraVideo(path, frameRate, width, height), autoRestart);
+			}
+			else
+			{
+				if(!path.empty() && !UDirectory::exists(path) && !UFile::exists(path))
+				{
+					ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
+				}
+				//usb device
+				camera_ = new rtabmap::CameraThread(new rtabmap::CameraVideo(deviceId, frameRate, width, height), autoRestart);
+			}
+			init();
+			if(!pause)
+			{
+				start();
+			}
+		}
+	}
+
+protected:
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("CameraEvent") == 0)
+		{
+			rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event;
+			const cv::Mat & image = e->image().image();
+			if(!image.empty() && image.depth() == CV_8U)
+			{
+				cv_bridge::CvImage img;
+				if(image.channels() == 1)
+				{
+					img.encoding = sensor_msgs::image_encodings::MONO8;
+				}
+				else
+				{
+					img.encoding = sensor_msgs::image_encodings::BGR8;
+				}
+				img.image = image;
+				sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
+				rosMsg->header.frame_id = frameId_;
+				rosMsg->header.stamp = ros::Time::now();
+				rosPublisher_.publish(rosMsg);
+			}
+		}
+	}
+
+private:
+	image_transport::Publisher rosPublisher_;
+	rtabmap::CameraThread * camera_;
+	ros::ServiceServer startSrv_;
+	ros::ServiceServer stopSrv_;
+	std::string frameId_;
+};
+
+CameraWrapper * camera = 0;
+void callback(rtabmap::CameraConfig &config, uint32_t level)
+{
+	if(camera)
+	{
+		camera->setParameters(config.device_id, config.frame_rate, config.width, config.height, config.video_or_images_path, config.auto_restart, config.pause);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+	ULogger::setEventLevel(ULogger::kWarning);
+
+	ros::init(argc, argv, "camera");
+
+	ros::NodeHandle nh("~");
+
+	camera = new CameraWrapper(); // webcam device 0
+
+	dynamic_reconfigure::Server<rtabmap::CameraConfig> server;
+	dynamic_reconfigure::Server<rtabmap::CameraConfig>::CallbackType f;
+	f = boost::bind(&callback, _1, _2);
+	server.setCallback(f);
+
+	ros::spin();
+
+	//cleanup
+	if(camera)
+	{
+		delete camera;
+	}
+
+	return 0;
+}

rtabmap/src/CoreNode.cpp

@@ -7,6 +7,8 @@
 
 #include "CoreWrapper.h"
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UStl.h>
 
 int main(int argc, char** argv)
 {
@@ -24,16 +26,63 @@ int main(int argc, char** argv)
 		{
 			deleteDbOnStart = true;
 		}
-		else if(!strcmp(argv[i], "--udebug"))
+		else if(strcmp(argv[i], "--udebug") == 0)
 		{
 			ULogger::setLevel(ULogger::kDebug);
 		}
+		else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
+		{
+			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+			uInsert(parameters,
+					std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(),
+					UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
+			uInsert(parameters,
+					std::make_pair(rtabmap::Parameters::kRtabmapDatabasePath(),
+					UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName())); // change default to ~/.ros
+
+			if(strcmp(argv[i], "--params") == 0)
+			{
+				// hide specific parameters
+				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
+				{
+					if(uSplit(iter->first, '/').front().compare("Bayes") == 0 ||
+					   uSplit(iter->first, '/').front().compare("VhEp") == 0 ||
+					   uSplit(iter->first, '/').front().compare("Odom") == 0 ||
+					   uSplit(iter->first, '/').front().compare("OdomBin") == 0 ||
+					   uSplit(iter->first, '/').front().compare("OdomICP") == 0)
+					{
+						parameters.erase(iter++);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+
+			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default RTAB-Map parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
 	}
 
-	CoreWrapper rtabmap(deleteDbOnStart);
+	CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart);
 
-	ROS_INFO("RTAB-Map started...");
+	ROS_INFO("rtabmap started...");
 	ros::spin();
 
+	delete rtabmap;
+
 	return 0;
 }

rtabmap/src/CoreWrapper.cpp

@@ -11,9 +11,9 @@
 #include <sensor_msgs/image_encodings.h>
 #include <cv_bridge/cv_bridge.h>
 #include <rtabmap/core/RtabmapEvent.h>
-#include <rtabmap/core/Rtabmap.h>
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/util3d.h>
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
@@ -21,154 +21,680 @@
 #include <rtabmap/utilite/UConversion.h>
 #include <opencv2/highgui/highgui.hpp>
 
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <laser_geometry/laser_geometry.h>
+
 //msgs
 #include "rtabmap/Info.h"
 #include "rtabmap/InfoEx.h"
+#include "rtabmap/MapData.h"
+
+#include "rtabmap/MsgConversion.h"
 
 using namespace rtabmap;
 
 CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
-		rtabmap_(0),
-		configPath_(UDirectory::homeDir()+"/.rtabmap/rtabmap.ini")
+		paused_(false),
+		frameId_("base_link"),
+		mapFrameId_("map"),
+		odomFrameId_(""),
+		configPath_(""),
+		mapToOdom_(tf::Transform::getIdentity()),
+		rate_(Parameters::defaultRtabmapDetectionRate()),
+		time_(ros::Time::now())
 {
-	ros::NodeHandle nh("~");
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+
+	bool subscribeLaserScan = false;
+	bool subscribeDepth = true;
+	int queueSize = 10;
+	double tfDelay = 0.05; // 20 Hz
+
+	// ROS related parameters (private)
+	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
+	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+
+	pnh.param("config_path", configPath_, configPath_);
+
+	pnh.param("frame_id", frameId_, frameId_);
+	pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
+	pnh.param("queue_size", queueSize, queueSize);
+
+	pnh.param("tf_delay", tfDelay, tfDelay);
+
+	ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
+	ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
+	ROS_INFO("rtabmap: queue_size = %d", queueSize);
+	ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
+
 	infoPub_ = nh.advertise<rtabmap::Info>("info", 1);
 	infoPubEx_ = nh.advertise<rtabmap::InfoEx>("infoEx", 1);
-	parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
-
-	rtabmap_ = new Rtabmap();
-
-	std::string workingDir = UDirectory::homeDir()+"/.rtabmap";
-	nh.param("config_path", configPath_, configPath_);
-	nh.param("working_directory", workingDir, workingDir);
+	mapData_ = nh.advertise<rtabmap::MapData>("mapData", 1);
 
 	configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
-	workingDir = uReplaceChar(workingDir, '~', UDirectory::homeDir());
 
-	loadNodeParameters(configPath_, workingDir);
+	// load parameters
+	ParametersMap parameters = loadParameters(configPath_);
 
-	rtabmap_->init(configPath_, deleteDbOnStart);
+	// update parameters with user input parameters (private)
+	uInsert(parameters, std::make_pair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
+	uInsert(parameters, std::make_pair(Parameters::kRtabmapDatabasePath(), UDirectory::homeDir()+"/.ros/"+Parameters::getDefaultDatabaseName())); // change default to ~/.ros
+	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string vStr;
+		bool vBool;
+		int vInt;
+		double vDouble;
+		if(pnh.getParam(iter->first, vStr))
+		{
+			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+			iter->second = vStr;
 
-	resetMemorySrv_ = nh.advertiseService("resetMemory", &CoreWrapper::resetMemoryCallback, this);
-	dumpMemorySrv_ = nh.advertiseService("dumpMemory", &CoreWrapper::dumpMemoryCallback, this);
-	deleteMemorySrv_ = nh.advertiseService("deleteMemory", &CoreWrapper::deleteMemoryCallback, this);
-	dumpPredictionSrv_ = nh.advertiseService("dumpPrediction", &CoreWrapper::dumpPredictionCallback, this);
+			if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
+			{
+				iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
+			}
+			else if(iter->first.compare(Parameters::kRtabmapDatabasePath()) == 0)
+			{
+				iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
+			}
+			else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0)
+			{
+				iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
+			}
+		}
+		else if(pnh.getParam(iter->first, vBool))
+		{
+			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+			iter->second = uBool2Str(vBool);
+		}
+		else if(pnh.getParam(iter->first, vInt))
+		{
+			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+			iter->second = uNumber2Str(vInt);
+		}
+		else if(pnh.getParam(iter->first, vDouble))
+		{
+			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+			iter->second = uNumber2Str(vDouble);
+		}
+	}
 
-	nh = ros::NodeHandle();
-	parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
+	// set public parameters
+	nh.param("is_rtabmap_paused", paused_, paused_);
+	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		nh.setParam(iter->first, iter->second);
+	}
+	if(parameters.find(Parameters::kRtabmapDetectionRate()) != parameters.end())
+	{
+		rate_ = std::atof(parameters.at(Parameters::kRtabmapDetectionRate()).c_str());
+		ROS_INFO("RTAB-Map rate detection = %f Hz", rate_);
+	}
+	bool isRGBD = uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str());
+	if(isRGBD)
+	{
+		// RGBD SLAM
+		if(!subscribeDepth && !subscribeLaserScan)
+		{
+			ROS_WARN("ROS param subscribe_depth and subscribe_laserScan are false, but RTAB-Map "
+					  "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth and subscribe_laserScan "
+					  "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure "
+					  "detection on images-only.");
+		}
+	}
+	else
+	{
+		// loop closure detection (images-only)
+		if(subscribeDepth || subscribeLaserScan)
+		{
+			ROS_WARN("ROS param subscribe_depth or subscribe_laserScan is true, but RTAB-Map "
+					  "parameter \"RGBD/Enabled\" is false! Please set subscribe_depth and subscribe_laserScan "
+					  "to false to use rtabmap node for loop closure detection on images-only, or set \"RGBD/Enabled\" to true "
+					  "for RGB-D SLAM.");
+		}
+	}
+	if(paused_)
+	{
+		UWARN("Node paused... dont' forget to call service \"pause_rtabmap\" to start rtabmap.");
+	}
 
-	image_transport::ImageTransport it(nh);
-	imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
+	// Init RTAB-Map
+	rtabmap_.init(parameters, deleteDbOnStart);
+
+	ROS_INFO("rtabmap: using database from \"%s\".", rtabmap_.getDatabasePath().c_str());
+
+	// setup services
+	updateSrv_ = nh.advertiseService("update_parameters", &CoreWrapper::updateRtabmapCallback, this);
+	resetSrv_ = nh.advertiseService("reset", &CoreWrapper::resetRtabmapCallback, this);
+	pauseSrv_ = nh.advertiseService("pause", &CoreWrapper::pauseRtabmapCallback, this);
+	resumeSrv_ = nh.advertiseService("resume", &CoreWrapper::resumeRtabmapCallback, this);
+	triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
+	publishMapDataSrv_ = nh.advertiseService("publish_map_data", &CoreWrapper::publishMapDataCallback, this);
+
+
+	setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
+
+	transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
 }
 
 CoreWrapper::~CoreWrapper()
 {
-	this->saveNodeParameters(configPath_);
-	delete rtabmap_;
+	if(transformThread_)
+	{
+		transformThread_->join();
+		delete transformThread_;
+	}
+
+	if(scanSync_)
+		delete scanSync_;
+	if(depthSync_)
+		delete depthSync_;
+	if(depthScanSync_)
+		delete depthScanSync_;
+
+	this->saveParameters(configPath_);
+
+	std::string databasePath = rtabmap_.getDatabasePath();
+	printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath.c_str());
 }
 
-ParametersMap CoreWrapper::loadNodeParameters(const std::string & configFile,
-											  const std::string & workingDirectory)
+ParametersMap CoreWrapper::loadParameters(const std::string & configFile)
 {
-	ROS_INFO("Loading parameters from %s", configFile.c_str());
-	if(!UFile::exists(configFile.c_str()))
-	{
-		ROS_WARN("Config file doesn't exist! It will be generated...");
-	}
-
 	ParametersMap parameters = Parameters::getDefaultParameters();
-	Rtabmap::readParameters(configFile.c_str(), parameters);
-
-	if(workingDirectory.size())
+	if(!configFile.empty())
 	{
-		parameters.at(Parameters::kRtabmapWorkingDirectory()) = workingDirectory;
+		ROS_INFO("Loading parameters from %s", configFile.c_str());
+		if(!UFile::exists(configFile.c_str()))
+		{
+			ROS_WARN("Config file doesn't exist! It will be generated...");
+		}
+		Rtabmap::readParameters(configFile.c_str(), parameters);
 	}
+	// otherwise take default parameters
 
-	ros::NodeHandle nh("~");
-	for(ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
-	{
-		nh.setParam(i->first, i->second);
-	}
-	parametersLoadedPub_.publish(std_msgs::Empty());
 	return parameters;
 }
 
-void CoreWrapper::saveNodeParameters(const std::string & configFile)
+void CoreWrapper::saveParameters(const std::string & configFile)
 {
-	printf("Saving parameters to %s\n", configFile.c_str());
-
-	if(!UFile::exists(configFile.c_str()))
+	if(!configFile.empty())
 	{
-		printf("Config file doesn't exist, a new one will be created.\n");
-	}
+		printf("Saving parameters to %s\n", configFile.c_str());
 
-	ParametersMap parameters = Parameters::getDefaultParameters();
-	ros::NodeHandle nh("~");
-	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-	{
-		std::string value;
-		if(nh.getParam(iter->first,value))
+		if(!UFile::exists(configFile.c_str()))
 		{
-			iter->second = value;
+			printf("Config file doesn't exist, a new one will be created.\n");
+		}
+
+		ParametersMap parameters = Parameters::getDefaultParameters();
+		ros::NodeHandle nh;
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			std::string value;
+			if(nh.getParam(iter->first,value))
+			{
+				iter->second = value;
+			}
+		}
+
+		Rtabmap::writeParameters(configFile.c_str(), parameters);
+	}
+	else
+	{
+		ROS_INFO("Parameters are not saved! (No configuration file provided...)");
+	}
+}
+
+void CoreWrapper::publishLoop(double tfDelay)
+{
+	if(tfDelay == 0)
+		return;
+	ros::Rate r(1.0 / tfDelay);
+	while(ros::ok())
+	{
+		if(!odomFrameId_.empty())
+		{
+			mapToOdomMutex_.lock();
+			ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
+			tfBroadcaster_.sendTransform( tf::StampedTransform (mapToOdom_, tfExpiration, mapFrameId_, odomFrameId_));
+			mapToOdomMutex_.unlock();
+		}
+		r.sleep();
+	}
+}
+
+void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
+{
+	if(!paused_)
+	{
+		if(rate_>0.0f)
+		{
+			if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
+			{
+				return;
+			}
+		}
+		time_ = ros::Time::now();
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		process(ptrImage->header.seq,
+				ptrImage->image);
+	}
+}
+
+void CoreWrapper::depthCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	if(!paused_)
+	{
+		if(rate_>0.0f)
+		{
+			if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
+			{
+				return;
+			}
+		}
+		time_ = ros::Time::now();
+
+		// TF ready?
+		Transform localTransform;
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+		process(ptrImage->header.seq,
+				ptrImage->image,
+				odom,
+				odomMsg->header.frame_id,
+				ptrDepth->image,
+				depthConstant,
+				localTransform,
+				cv::Mat());
+	}
+}
+
+void CoreWrapper::scanCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	if(!paused_)
+	{
+		if(rate_>0.0f)
+		{
+			if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
+			{
+				return;
+			}
+		}
+		time_ = ros::Time::now();
+
+		// TF ready?
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		//transform in frameId_ frame
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		pcl::PointCloud<pcl::PointXYZ> pclScan;
+		pcl::fromROSMsg(scanOut, pclScan);
+		cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+
+		process(ptrImage->header.seq,
+				ptrImage->image,
+				odom,
+				odomMsg->header.frame_id,
+				cv::Mat(),
+				0.0f,
+				Transform(),
+				scan);
+	}
+}
+
+void CoreWrapper::depthScanCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	if(!paused_)
+	{
+		if(rate_>0.0f)
+		{
+			if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
+			{
+				return;
+			}
+		}
+		time_ = ros::Time::now();
+
+		// TF ready?
+		Transform localTransform;
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
+			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		//transform in frameId_ frame
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		pcl::PointCloud<pcl::PointXYZ> pclScan;
+		pcl::fromROSMsg(scanOut, pclScan);
+		cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+		process(ptrImage->header.seq,
+				ptrImage->image,
+				odom,
+				odomMsg->header.frame_id,
+				ptrDepth->image,
+				depthConstant,
+				localTransform,
+				scan);
+	}
+}
+
+void CoreWrapper::process(
+		int id,
+		const cv::Mat & image,
+		const Transform & odom,
+		const std::string & odomFrameId,
+		const cv::Mat & depth,
+		float depthConstant,
+		const Transform & localTransform,
+		const cv::Mat & scan)
+{
+	ROS_INFO("rtabmap: Processing data...");
+	if(rtabmap_.isIDsGenerated() || id > 0)
+	{
+		cv::Mat depth16;
+		if(!depth.empty() && depth.type() != CV_16UC1)
+		{
+			if(depth.type() == CV_32FC1)
+			{
+				//convert to 16 bits
+				depth16 = util3d::cvtDepthFromFloat(depth);
+				static bool shown = false;
+				if(!shown)
+				{
+					ROS_WARN("Use depth image with \"unsigned short\" type to "
+							 "avoid conversion. This message is only printed once...");
+					shown = true;
+				}
+			}
+			else
+			{
+				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
+				return;
+			}
+		}
+		else
+		{
+			depth16 = depth;
+		}
+
+		Image data(image,
+				depth16,
+				scan,
+				depthConstant,
+				odom,
+				localTransform,
+				id);
+
+		if(!rtabmap_.process(data))
+		{
+			ROS_WARN("RTAB-Map could not process the data received! (ROS id = %d)", id);
+		}
+		else
+		{
+			mapToOdomMutex_.lock();
+			rtabmap::transformToTF(rtabmap_.getMapCorrection(), mapToOdom_);
+			odomFrameId_ = odomFrameId;
+			mapToOdomMutex_.unlock();
+
+			const Statistics & stats = rtabmap_.getStatistics();
+			this->publishStats(stats);
 		}
 	}
-
-	Rtabmap::writeParameters(configFile.c_str(), parameters);
-
-	std::string databasePath = parameters.at(Parameters::kRtabmapWorkingDirectory())+"/rtabmap.db";
-	printf("Saving database/long-term memory... (located at %s)\n", databasePath.c_str());
-}
-
-void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
-{
-	if(msg->data.size())
+	else if(!rtabmap_.isIDsGenerated())
 	{
-		//ROS_INFO("Received image.");
-		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
-		rtabmap_->process(ptr->image);
-		const Statistics & stats = rtabmap_->getStatistics();
-		this->publishStats(stats);
+		ROS_WARN("Ignoring received image because its sequence ID=0. Please "
+				 "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
+				 "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
+				 "when you need to have IDs output of RTAB-map synchronised with the source "
+				 "image sequence ID.");
 	}
 }
 
-bool CoreWrapper::resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-{
-	rtabmap_->resetMemory();
-	return true;
-}
-
-bool CoreWrapper::dumpMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-{
-	rtabmap_->dumpData();
-	return true;
-}
-
-bool CoreWrapper::deleteMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-{
-	rtabmap_->resetMemory(true);
-	return true;
-}
-
-bool CoreWrapper::dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-{
-	rtabmap_->dumpPrediction();
-	return true;
-}
-
-void CoreWrapper::parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
+bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
-	ros::NodeHandle nh("~");
+	ros::NodeHandle nh;
 	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
-		std::string value;
-		if(nh.getParam(iter->first, value))
+		std::string vStr;
+		bool vBool;
+		int vInt;
+		double vDouble;
+		if(nh.getParam(iter->first, vStr))
 		{
-			iter->second = value;
+			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+			iter->second = vStr;
+		}
+		else if(nh.getParam(iter->first, vBool))
+		{
+			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+			iter->second = uBool2Str(vBool);
+		}
+		else if(nh.getParam(iter->first, vInt))
+		{
+			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+			iter->second = uNumber2Str(vInt).c_str();
+		}
+		else if(nh.getParam(iter->first, vDouble))
+		{
+			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+			iter->second = uNumber2Str(vDouble).c_str();
 		}
 	}
-	ROS_INFO("Updating parameters");
-	rtabmap_->parseParameters(parameters);
+	ROS_INFO("rtabmap: Updating parameters");
+	if(parameters.find(Parameters::kRtabmapDetectionRate()) != parameters.end())
+	{
+		rate_ = std::atof(parameters.at(Parameters::kRtabmapDetectionRate()).c_str());
+		ROS_INFO("RTAB-Map rate detection = %f Hz", rate_);
+	}
+	rtabmap_.parseParameters(parameters);
+	return true;
+}
+
+bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	ROS_INFO("rtabmap: Reset");
+	rtabmap_.resetMemory(true);
+	return true;
+}
+
+bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(paused_)
+	{
+		ROS_WARN("rtabmap: Already paused!");
+	}
+	else
+	{
+		paused_ = true;
+		ROS_INFO("rtabmap: paused!");
+		ros::NodeHandle nh;
+		nh.setParam("is_rtabmap_paused", true);
+	}
+	return true;
+}
+
+bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(!paused_)
+	{
+		ROS_WARN("rtabmap: Already running!");
+	}
+	else
+	{
+		paused_ = false;
+		ROS_INFO("rtabmap: resumed!");
+		ros::NodeHandle nh;
+		nh.setParam("is_rtabmap_paused", false);
+	}
+	return true;
+}
+
+bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	ROS_INFO("rtabmap: Trigger new map");
+	rtabmap_.triggerNewMap();
+	return true;
+}
+
+bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	ROS_INFO("rtabmap: Publishing map data...");
+	std::map<int, std::vector<unsigned char> > images;
+	std::map<int, std::vector<unsigned char> > depths;
+	std::map<int, std::vector<unsigned char> > depths2d;
+	std::map<int, float> depthConstants;
+	std::map<int, Transform> localTransforms;
+	std::map<int, Transform> poses;
+	Transform mapCorrection;
+
+	if(mapData_.getNumSubscribers())
+	{
+		rtabmap::MapDataPtr msg(new rtabmap::MapData);
+		msg->header.stamp = ros::Time::now();
+
+		rtabmap_.get3DMap(images,
+				depths,
+				depths2d,
+				depthConstants,
+				localTransforms,
+				poses,
+				mapCorrection);
+
+		int i=0;
+
+		msg->imageIDs.resize(images.size());
+		msg->images.resize(images.size());
+		i=0;
+		for(std::map<int, std::vector<unsigned char> >::iterator iter = images.begin(); iter!=images.end(); ++iter)
+		{
+			msg->imageIDs[i] = iter->first;
+			msg->images[i].bytes = iter->second;
+			++i;
+		}
+
+		msg->depthIDs.resize(depths.size());
+		msg->depths.resize(depths.size());
+		i=0;
+		for(std::map<int, std::vector<unsigned char> >::iterator iter = depths.begin(); iter!=depths.end(); ++iter)
+		{
+			msg->depthIDs[i] = iter->first;
+			msg->depths[i].bytes = iter->second;
+			++i;
+		}
+
+		msg->depth2DIDs.resize(depths2d.size());
+		msg->depths2D.resize(depths2d.size());
+		i=0;
+		for(std::map<int, std::vector<unsigned char> >::iterator iter = depths2d.begin(); iter!=depths2d.end(); ++iter)
+		{
+			msg->depth2DIDs[i] = iter->first;
+			msg->depths2D[i].bytes = iter->second;
+			++i;
+		}
+
+		msg->depthConstantIDs.resize(depthConstants.size());
+		msg->depthConstants.resize(depthConstants.size());
+		i=0;
+		for(std::map<int, float>::iterator iter = depthConstants.begin(); iter!=depthConstants.end(); ++iter)
+		{
+			msg->depthConstantIDs[i] = iter->first;
+			msg->depthConstants[i] = iter->second;
+			++i;
+		}
+
+		msg->localTransformIDs.resize(localTransforms.size());
+		msg->localTransforms.resize(localTransforms.size());
+		i=0;
+		for(std::map<int, Transform>::iterator iter = localTransforms.begin(); iter!=localTransforms.end(); ++iter)
+		{
+			msg->localTransformIDs[i] = iter->first;
+			transformToGeometryMsg(iter->second, msg->localTransforms[i]);
+			++i;
+		}
+
+		msg->poseIDs.resize(poses.size());
+		msg->poses.resize(poses.size());
+		i=0;
+		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			msg->poseIDs[i] = iter->first;
+			transformToPoseMsg(iter->second, msg->poses[i]);
+			++i;
+		}
+
+		transformToGeometryMsg(mapCorrection, msg->mapCorrection);
+
+		mapData_.publish(msg);
+	}
+
+	return true;
 }
 
 void CoreWrapper::publishStats(const Statistics & stats)
@@ -179,8 +705,32 @@ void CoreWrapper::publishStats(const Statistics & stats)
 		{
 			//ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
 			rtabmap::InfoPtr msg(new rtabmap::Info);
+			msg->header.stamp = ros::Time::now();
+			msg->header.frame_id = mapFrameId_;
+
 			msg->refId = stats.refImageId();
+			msg->refMapId = stats.refImageMapId();
 			msg->loopClosureId = stats.loopClosureId();
+			msg->loopClosureMapId = stats.loopClosureMapId();
+			msg->localLoopClosureId = stats.localLoopClosureId();
+			msg->localLoopClosureMapId = stats.localLoopClosureMapId();
+
+			msg->nodeIds.resize(stats.poses().size());
+			msg->nodePoses.resize(stats.poses().size());
+			int i=0;
+			for(std::map<int, Transform>::const_iterator iter = stats.poses().begin();
+				iter!=stats.poses().end();
+				++iter)
+			{
+				msg->nodeIds[i] = iter->first;
+				transformToPoseMsg(iter->second, msg->nodePoses[i]);
+				++i;
+			}
+
+			transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection);
+			transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
+			transformToPoseMsg(stats.currentPose(), msg->currentPose);
+
 			infoPub_.publish(msg);
 		}
 
@@ -188,56 +738,37 @@ void CoreWrapper::publishStats(const Statistics & stats)
 		{
 			//ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId());
 			rtabmap::InfoExPtr msg(new rtabmap::InfoEx);
+			msg->header.stamp = ros::Time::now();
+			msg->header.frame_id = mapFrameId_;
+
 			msg->refId = stats.refImageId();
+			msg->refMapId = stats.refImageMapId();
 			msg->loopClosureId = stats.loopClosureId();
+			msg->loopClosureMapId = stats.loopClosureMapId();
+			msg->localLoopClosureId = stats.localLoopClosureId();
+			msg->localLoopClosureMapId = stats.localLoopClosureMapId();
+
+			msg->nodeIds.resize(stats.poses().size());
+			msg->nodePoses.resize(stats.poses().size());
+			int i=0;
+			for(std::map<int, Transform>::const_iterator iter = stats.poses().begin();
+				iter!=stats.poses().end();
+				++iter)
+			{
+				msg->nodeIds[i] = iter->first;
+				transformToPoseMsg(iter->second, msg->nodePoses[i]);
+				++i;
+			}
+
+			transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection);
+			transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
+			transformToPoseMsg(stats.currentPose(), msg->currentPose);
 
 			// Detailed info
 			if(stats.extended())
 			{
-				if(!stats.refImage().empty())
-				{
-					// compress (the gui would work on a remote computer, this on the robot)
-					cv::imencode(".png", stats.refImage(), msg->refImage);
-
-					/*
-					cv_bridge::CvImage img;
-					if(stats.refImage().channels() == 1)
-					{
-						img.encoding = sensor_msgs::image_encodings::MONO8;
-					}
-					else
-					{
-						img.encoding = sensor_msgs::image_encodings::BGR8;
-					}
-					img.image = stats.refImage();
-					sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
-					rosMsg->header.frame_id = "camera";
-					rosMsg->header.stamp = ros::Time::now();
-					msg->refImage = *rosMsg;
-					*/
-				}
-				if(!stats.loopImage().empty())
-				{
-					// compress (the gui would work on a remote computer, this on the robot)
-					cv::imencode(".png", stats.loopImage(), msg->loopImage);
-
-					/*
-					cv_bridge::CvImage img;
-					if(stats.loopImage().channels() == 1)
-					{
-						img.encoding = sensor_msgs::image_encodings::MONO8;
-					}
-					else
-					{
-						img.encoding = sensor_msgs::image_encodings::BGR8;
-					}
-					img.image = stats.loopImage();
-					sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
-					rosMsg->header.frame_id = "camera";
-					rosMsg->header.stamp = ros::Time::now();
-					msg->loopImage = *rosMsg;
-					*/
-				}
+				msg->refImage = stats.refImage();
+				msg->loopImage = stats.loopImage();
 
 				//Posterior, likelihood, childCount
 				msg->posteriorKeys = uKeys(stats.posterior());
@@ -287,8 +818,86 @@ void CoreWrapper::publishStats(const Statistics & stats)
 				// Statistics data
 				msg->statsKeys = uKeys(stats.data());
 				msg->statsValues = uValues(stats.data());
+
+				//RGB-D SLAM data
+				msg->refDepth = stats.refDepth();
+				msg->refDepth2D = stats.refDepth2D();
+				msg->loopDepth = stats.loopDepth();
+				msg->loopDepth2D = stats.loopDepth2D();
+
+				msg->refDepthConstant = stats.refDepthConstant();
+				msg->loopDepthConstant = stats.loopDepthConstant();
+
+				transformToGeometryMsg(stats.refLocalTransform(), msg->refLocalTransform);
+				transformToGeometryMsg(stats.loopLocalTransform(), msg->loopLocalTransform);
 			}
 			infoPubEx_.publish(msg);
 		}
 	}
 }
+
+/**
+ * exclusive callbacks:
+ *     image
+ *     image + depth
+ *     image + scan
+ *     image + depth + scan
+ * Which callback is called depends on
+ * the combination of these options:
+ *     bool subscribe_laserScan
+ *     bool subscribe_depth
+ */
+void CoreWrapper::setupCallbacks(
+		bool subscribeDepth,
+		bool subscribeLaserScan,
+		int queueSize)
+{
+	ros::NodeHandle nh; // public
+	ros::NodeHandle pnh("~"); // private
+	ros::NodeHandle rgb_nh(nh, "rgb");
+	ros::NodeHandle depth_nh(nh, "depth");
+	ros::NodeHandle rgb_pnh(pnh, "rgb");
+	ros::NodeHandle depth_pnh(pnh, "depth");
+	image_transport::ImageTransport rgb_it(rgb_nh);
+	image_transport::ImageTransport depth_it(depth_nh);
+	image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+	image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+	if(subscribeDepth && subscribeLaserScan)
+	{
+		ROS_INFO("Registering Depth+LaserScan callback...");
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+		odomSub_.subscribe(nh, "odom", 1);
+		scanSub_.subscribe(nh, "scan", 1);
+		depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+	}
+	else if(subscribeDepth && !subscribeLaserScan)
+	{
+		ROS_INFO("Registering Depth callback...");
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+		odomSub_.subscribe(nh, "odom", 1);
+		depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
+		depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
+	}
+	else if(!subscribeDepth && subscribeLaserScan)
+	{
+		ROS_INFO("Registering LaserScan callback...");
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		odomSub_.subscribe(nh, "odom", 1);
+		scanSub_.subscribe(nh, "scan", 1);
+		scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
+		scanSync_->registerCallback(boost::bind(&CoreWrapper::scanCallback, this, _1, _2, _3));
+	}
+	else
+	{
+		ROS_INFO("Registering default callback...");
+		defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
+	}
+}
+
+

rtabmap/src/CoreWrapper.h

@@ -10,19 +10,32 @@
 
 
 #include <ros/ros.h>
-#include <std_srvs/Empty.h>
-#include <std_msgs/Empty.h>
-#include <cv_bridge/cv_bridge.h>
-#include <rtabmap/core/Statistics.h>
-#include <sensor_msgs/Image.h>
-#include <geometry_msgs/Twist.h>
-#include <image_transport/image_transport.h>
-#include <rtabmap/core/Parameters.h>
 
-namespace rtabmap
-{
-	class Rtabmap;
-}
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+#include <std_srvs/Empty.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <std_msgs/Empty.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/LaserScan.h>
+#include <nav_msgs/Odometry.h>
+
+#include <rtabmap/core/Statistics.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Rtabmap.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
 
 class CoreWrapper
 {
@@ -31,36 +44,103 @@ public:
 	virtual ~CoreWrapper();
 
 private:
-	void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg);
-	void twistCallback(const geometry_msgs::TwistConstPtr & msg);
-	void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
+	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize);
+	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
+	void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					   const nav_msgs::OdometryConstPtr & odomMsg,
+					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void scanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					  const nav_msgs::OdometryConstPtr & odomMsg,
+					  const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+						   const nav_msgs::OdometryConstPtr & odomMsg,
+						   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+						   const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
+						   const sensor_msgs::LaserScanConstPtr& scanMsg);
 
-	bool resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool dumpMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool deleteMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	void process(
+			int id,
+			const cv::Mat & image,
+			const rtabmap::Transform & odom = rtabmap::Transform(),
+			const std::string & odomFrameId = "",
+			const cv::Mat & depth = cv::Mat(),
+			float depthConstant = 0.0f,
+			const rtabmap::Transform & localTransform = rtabmap::Transform(),
+			const cv::Mat & scan = cv::Mat());
 
-	rtabmap::ParametersMap loadNodeParameters(const std::string & configFile,
-											  const std::string & workingDirectory);
-	void saveNodeParameters(const std::string & configFile);
+	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
+	rtabmap::ParametersMap loadParameters(const std::string & configFile);
+	void saveParameters(const std::string & configFile);
+
+	void publishLoop(double tfDelay);
 
 	void publishStats(const rtabmap::Statistics & stats);
 
 private:
-	rtabmap::Rtabmap * rtabmap_;
-	image_transport::Subscriber imageTopic_;
-	ros::Subscriber audioFrameFreqSqrdMagnTopic_;
-	ros::Subscriber twistTopic_;
-	ros::Subscriber parametersUpdatedTopic_;
-	ros::Publisher infoPub_;
-	ros::Publisher infoPubEx_;
-	ros::Publisher parametersLoadedPub_;
+	rtabmap::Rtabmap rtabmap_;
+	bool paused_;
+
+	std::string frameId_;
+	std::string mapFrameId_;
+	std::string odomFrameId_;
 	std::string configPath_;
 
-	ros::ServiceServer resetMemorySrv_;
-	ros::ServiceServer dumpMemorySrv_;
-	ros::ServiceServer deleteMemorySrv_;
-	ros::ServiceServer dumpPredictionSrv_;
+	tf::Transform mapToOdom_;
+	boost::mutex mapToOdomMutex_;
+
+	ros::Publisher infoPub_;
+	ros::Publisher infoPubEx_;
+	ros::Publisher mapData_;
+
+	image_transport::Subscriber defaultSub_;
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
+	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
+	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
+	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::LaserScan> MyScanSyncPolicy;
+	message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
+
+	tf::TransformBroadcaster tfBroadcaster_;
+	tf::TransformListener tfListener_;
+
+	ros::ServiceServer updateSrv_;
+	ros::ServiceServer resetSrv_;
+	ros::ServiceServer pauseSrv_;
+	ros::ServiceServer resumeSrv_;
+	ros::ServiceServer triggerNewMapSrv_;
+	ros::ServiceServer publishMapDataSrv_;
+
+	boost::thread* transformThread_;
+
+	float rate_;
+	ros::Time time_;
 };
 
 #endif /* COREWRAPPER_H_ */

rtabmap/src/DataRecorderNode.cpp

@@ -0,0 +1,440 @@
+
+#include <ros/ros.h>
+
+#include "rtabmap/utilite/ULogger.h"
+
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/LaserScan.h>
+#include <nav_msgs/Odometry.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include "rtabmap/MsgConversion.h"
+
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <laser_geometry/laser_geometry.h>
+
+#include <rtabmap/core/util3d.h>
+
+#include <QtGui/QApplication>
+
+#include "rtabmap/gui/DataRecorder.h"
+
+using namespace rtabmap;
+
+
+class DataRecorderWrapper
+{
+public:
+	DataRecorderWrapper() :
+		fileName_(""),
+		frameId_("base_link")
+	{
+		ros::NodeHandle pnh("~");
+
+		bool subscribeOdometry = false;
+		bool subscribeLaserScan = false;
+		bool subscribeDepth = false;
+		int queueSize = 10;
+		pnh.param("subscribe_odometry", subscribeOdometry, subscribeOdometry);
+		pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
+		pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("output_file_name", fileName_, fileName_);
+		pnh.param("frame_id", frameId_, frameId_);
+
+		setupCallbacks(subscribeOdometry, subscribeDepth, subscribeLaserScan, queueSize);
+	}
+	bool init()
+	{
+		return recorder_.init(fileName_.c_str());
+	}
+
+private:
+	void setupCallbacks(
+			bool subscribeOdom,
+			bool subscribeDepth,
+			bool subscribeLaserScan,
+			int queueSize)
+	{
+		ros::NodeHandle nh; // public
+		ros::NodeHandle pnh("~"); // private
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		if(subscribeOdom && subscribeDepth && subscribeLaserScan)
+		{
+			ROS_INFO("Registering Depth+LaserScan callback...");
+			imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+			cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+			odomSub_.subscribe(nh, "odom", 1);
+			scanSub_.subscribe(nh, "scan", 1);
+			depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			depthScanSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+		}
+		else if(subscribeOdom && subscribeDepth && !subscribeLaserScan)
+		{
+			ROS_INFO("Registering Depth callback...");
+			imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+			cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+			odomSub_.subscribe(nh, "odom", 1);
+			depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
+			depthSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthCallback, this, _1, _2, _3, _4));
+		}
+		else if(subscribeOdom && !subscribeDepth && subscribeLaserScan)
+		{
+			ROS_INFO("Registering LaserScan callback...");
+			imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			odomSub_.subscribe(nh, "odom", 1);
+			scanSub_.subscribe(nh, "scan", 1);
+			scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
+			scanSync_->registerCallback(boost::bind(&DataRecorderWrapper::scanCallback, this, _1, _2, _3));
+		}
+		else if(!subscribeOdom && subscribeDepth)
+		{
+			ROS_INFO("Registering to depth without odometry callback...");
+			imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+			cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+			depthImageSync_ = new message_filters::Synchronizer<MyDepthImageSyncPolicy>(MyDepthImageSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			depthImageSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthImageCallback, this, _1, _2, _3));
+		}
+		else
+		{
+			ROS_INFO("Registering default callback...");
+			defaultSub_ = rgb_it.subscribe("image", 1, &DataRecorderWrapper::defaultCallback, this);
+		}
+	}
+
+	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
+	{
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			cv::Mat(),
+			cv::Mat(),
+			0.0f,
+			Transform(),
+			Transform());
+		recorder_.addData(image);
+	}
+
+	void depthImageCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+	{
+		// TF ready?
+		Transform localTransform;
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+		cv::Mat depth16;
+		if(ptrDepth->image.type() != CV_16UC1)
+		{
+			if(ptrDepth->image.type() == CV_32FC1)
+			{
+				//convert to 16 bits
+				depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
+				static bool shown = false;
+				if(!shown)
+				{
+					ROS_WARN("Use depth image with \"unsigned short\" type to "
+							 "avoid conversion. This message is only printed once...");
+					shown = true;
+				}
+			}
+			else
+			{
+				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
+				return;
+			}
+		}
+		else
+		{
+			depth16 = ptrDepth->image;
+		}
+
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			depth16,
+			cv::Mat(),
+			depthConstant,
+			Transform(),
+			localTransform);
+		recorder_.addData(image);
+	}
+
+	void depthCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+	{
+		// TF ready?
+		Transform localTransform;
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+		cv::Mat depth16;
+		if(ptrDepth->image.type() != CV_16UC1)
+		{
+			if(ptrDepth->image.type() == CV_32FC1)
+			{
+				//convert to 16 bits
+				depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
+				static bool shown = false;
+				if(!shown)
+				{
+					ROS_WARN("Use depth image with \"unsigned short\" type to "
+							 "avoid conversion. This message is only printed once...");
+					shown = true;
+				}
+			}
+			else
+			{
+				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
+				return;
+			}
+		}
+		else
+		{
+			depth16 = ptrDepth->image;
+		}
+
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			depth16,
+			cv::Mat(),
+			depthConstant,
+			odom,
+			localTransform);
+		recorder_.addData(image);
+	}
+
+	void scanCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::LaserScanConstPtr& scanMsg)
+	{
+		// TF ready?
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		//transform in frameId_ frame
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		pcl::PointCloud<pcl::PointXYZ> pclScan;
+		pcl::fromROSMsg(scanOut, pclScan);
+		cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			cv::Mat(),
+			scan,
+			0.0f,
+			odom,
+			Transform());
+		recorder_.addData(image);
+
+	}
+
+	void depthScanCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+			const sensor_msgs::LaserScanConstPtr& scanMsg)
+	{
+		// TF ready?
+		Transform localTransform;
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
+			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		//transform in frameId_ frame
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		pcl::PointCloud<pcl::PointXYZ> pclScan;
+		pcl::fromROSMsg(scanOut, pclScan);
+		cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+		cv::Mat depth16;
+		if(ptrDepth->image.type() != CV_16UC1)
+		{
+			if(ptrDepth->image.type() == CV_32FC1)
+			{
+				//convert to 16 bits
+				depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
+				static bool shown = false;
+				if(!shown)
+				{
+					ROS_WARN("Use depth image with \"unsigned short\" type to "
+							 "avoid conversion. This message is only printed once...");
+					shown = true;
+				}
+			}
+			else
+			{
+				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
+				return;
+			}
+		}
+		else
+		{
+			depth16 = ptrDepth->image;
+		}
+
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			depth16,
+			scan,
+			depthConstant,
+			odom,
+			localTransform);
+		recorder_.addData(image);
+	}
+
+private:
+	DataRecorder recorder_;
+	std::string fileName_;
+	std::string frameId_;
+
+	image_transport::Subscriber defaultSub_;
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
+	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
+	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
+	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::LaserScan> MyScanSyncPolicy;
+	message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
+
+	//without odom
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthImageSyncPolicy;
+	message_filters::Synchronizer<MyDepthImageSyncPolicy> * depthImageSync_;
+
+	tf::TransformListener tfListener_;
+};
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	ros::init(argc, argv, "data_recorder");
+
+	QApplication app(argc, argv);
+
+	DataRecorderWrapper recorder;
+
+	if(recorder.init())
+	{
+		ros::spin();
+	}
+	else
+	{
+		ROS_ERROR("Cannot initialize the recorder! Make sure the parameter output_file_name is set!");
+	}
+}

rtabmap/src/GridMapAssemblerNode.cpp

@@ -0,0 +1,267 @@
+/*
+ * GuiWrapper.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include <QtGui/QApplication>
+#include <QtCore/QDir>
+
+#include <cv_bridge/cv_bridge.h>
+#include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
+#include <nav_msgs/GetMap.h>
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <rtabmap/gui/MainWindow.h>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/ParamEvent.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/util3d.h>
+
+#include "rtabmap/MsgConversion.h"
+
+#include "PreferencesDialogROS.h"
+
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/common/common.h>
+#include <laser_geometry/laser_geometry.h>
+
+using namespace rtabmap;
+
+class GridMapAssembler
+{
+
+public:
+	GridMapAssembler() :
+		scanVoxelSize_(0.01)
+	{
+		ros::NodeHandle pnh("~");
+		pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+
+		ros::NodeHandle nh;
+		infoExTopic_ = nh.subscribe("infoEx", 1, &GridMapAssembler::infoExReceivedCallback, this);
+		mapDataTopic_ = nh.subscribe("mapData", 1, &GridMapAssembler::mapDataReceivedCallback, this);
+
+		gridMap_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
+		getMapService_ = nh.advertiseService("get_map", &GridMapAssembler::getMapCallback, this);
+	}
+
+	~GridMapAssembler()
+	{
+	}
+
+	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
+	{
+		if(!uContains(scans_, msg->refId) && msg->refDepth2D.size())
+		{
+			cv::Mat depth2d = util3d::uncompressData(msg->refDepth2D);
+			scans_.insert(std::make_pair(msg->refId, util3d::depth2DToPointCloud(depth2d)));
+		}
+
+		std::map<int, Transform> poses;
+		for(unsigned int i=0; i<msg->nodeIds.size() && i<msg->nodePoses.size(); ++i)
+		{
+			poses.insert(std::make_pair(msg->nodeIds[i], transformFromPoseMsg(msg->nodePoses[i])));
+		}
+
+		if(gridMap_.getNumSubscribers())
+		{
+			float delta = 0.05; //m
+
+			// create the map
+			float xMin=0.0f, yMin=0.0f;
+			cv::Mat pixels = create2DMap(poses, delta, xMin, yMin);
+
+			if(!pixels.empty())
+			{
+				//init
+				map_.info.resolution = delta;
+				map_.info.origin.position.x = 0.0;
+				map_.info.origin.position.y = 0.0;
+				map_.info.origin.position.z = 0.0;
+				map_.info.origin.orientation.x = 0.0;
+				map_.info.origin.orientation.y = 0.0;
+				map_.info.origin.orientation.z = 0.0;
+				map_.info.origin.orientation.w = 1.0;
+
+				map_.info.width = pixels.cols;
+				map_.info.height = pixels.rows;
+				map_.info.origin.position.x = xMin;
+				map_.info.origin.position.y = yMin;
+				map_.data.resize(map_.info.width * map_.info.height);
+
+				memcpy(map_.data.data(), pixels.data, map_.info.width * map_.info.height);
+
+				map_.header.frame_id = msg->header.frame_id;
+				map_.header.stamp = ros::Time::now();
+
+				gridMap_.publish(map_);
+			}
+		}
+	}
+
+	void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
+	{
+		std::map<int, std::vector<unsigned char> > depths2d;
+
+		if(msg->depth2DIDs.size() != msg->depths2D.size())
+		{
+			ROS_WARN("grid_map_assembler: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!",
+					(int)msg->depths2D.size(), (int)msg->depth2DIDs.size());
+		}
+
+		// fill maps
+		for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depths2D.size(); ++i)
+		{
+			if(!uContains(scans_, msg->depth2DIDs[i]))
+			{
+				cv::Mat depth2d = util3d::uncompressData(msg->depths2D[i].bytes);
+				scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d)));
+			}
+		}
+	}
+
+	cv::Mat create2DMap(const std::map<int, Transform> & poses, float delta, float & xMin, float & yMin)
+	{
+		std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans;
+
+		pcl::PointCloud<pcl::PointXYZ> minMax;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(uContains(scans_, iter->first))
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(scans_.at(iter->first), iter->second);
+				pcl::PointXYZ min, max;
+				pcl::getMinMax3D(*cloud, min, max);
+				minMax.push_back(min);
+				minMax.push_back(max);
+				minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+				scans.insert(std::make_pair(iter->first, cloud));
+			}
+		}
+
+		cv::Mat map;
+		if(minMax.size())
+		{
+			//Get map size
+			pcl::PointXYZ min, max;
+			pcl::getMinMax3D(minMax, min, max);
+
+			xMin = min.x-1.0f;
+			yMin = min.y-1.0f;
+			float xMax = max.x+1.0f;
+			float yMax = max.y+1.0f;
+
+			map = cv::Mat::ones((yMax - yMin) / delta, (xMax - xMin) / delta, CV_8S)*-1;
+			for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = scans.begin(); iter!=scans.end(); ++iter)
+			{
+				for(unsigned int i=0; i<iter->second->size(); ++i)
+				{
+					const Transform & pose = poses.at(iter->first);
+					cv::Point2i start((pose.x()-xMin)/delta + 0.5f, (pose.y()-yMin)/delta + 0.5f);
+					cv::Point2i end((iter->second->points[i].x-xMin)/delta + 0.5f, (iter->second->points[i].y-yMin)/delta + 0.5f);
+
+					rayTrace(start, end, map); // trace free space
+
+					map.at<char>(end.y, end.x) = 100; // obstacle
+				}
+			}
+		}
+		return map;
+	}
+
+	void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid)
+	{
+		UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
+		UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
+		UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
+		UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
+
+		cv::Point2i ptA, ptB;
+		if(start.x > end.x)
+		{
+			ptA = end;
+			ptB = start;
+		}
+		else
+		{
+			ptA = start;
+			ptB = end;
+		}
+
+		float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
+		float b = ptA.y - slope*ptA.x;
+
+
+		//ROS_WARN("start=%d,%d end=%d,%d", ptA.x, ptA.y, ptB.x, ptB.y);
+
+		//ROS_WARN("y = %f*x + %f", slope, b);
+
+		for(int x=ptA.x; x<ptB.x; ++x)
+		{
+			float lowerbound = float(x)*slope + b;
+			float upperbound = float(x+1)*slope + b;
+
+			if(lowerbound > upperbound)
+			{
+				float tmp = lowerbound;
+				lowerbound = upperbound;
+				upperbound = tmp;
+			}
+
+			//ROS_WARN("lowerbound=%f upperbound=%f", lowerbound, upperbound);
+			UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f", lowerbound, grid.cols, x, slope, b).c_str());
+			UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f", upperbound, grid.cols, x+1, slope, b).c_str());
+			for(int y = lowerbound; y<=(int)upperbound; ++y)
+			{
+				if(grid.at<char>(y, x) == -1)
+				{
+					grid.at<char>(y, x) = 0; // free space
+				}
+			}
+		}
+	}
+
+	bool getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
+	{
+		if(map_.data.size())
+		{
+			res.map = map_;
+			return true;
+		}
+		return false;
+	}
+
+private:
+	double scanVoxelSize_;
+
+	ros::Subscriber infoExTopic_;
+	ros::Subscriber mapDataTopic_;
+
+	ros::Publisher gridMap_;
+
+	ros::ServiceServer getMapService_;
+
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
+
+	nav_msgs::OccupancyGrid map_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "grid_map_assembler");
+	GridMapAssembler assembler;
+	ros::spin();
+	return 0;
+}

rtabmap/src/GuiNode.cpp

@@ -18,7 +18,7 @@ void my_handler(int s){
 
 int main(int argc, char** argv)
 {
-	ros::init(argc, argv, "rtabmap_gui");
+	ros::init(argc, argv, "rtabmapviz");
 
 	GuiWrapper gui(argc, argv);
 
@@ -34,12 +34,12 @@ int main(int argc, char** argv)
 	ros::AsyncSpinner spinner(4); // Use 4 threads
 	spinner.start();
 
-	ROS_INFO("Node started.");
+	ROS_INFO("rtabmapviz started.");
 	// Now wait for application to finish
 	int r = gui.exec();// MUST be called by the Main Thread
 
 	spinner.stop();
 
-	ROS_INFO("All done! Closing...");
+	ROS_INFO("rtabmapviz: All done! Closing...");
 	return r;
 }

rtabmap/src/GuiWrapper.cpp

@@ -7,12 +7,14 @@
 
 #include "GuiWrapper.h"
 #include <QtGui/QApplication>
+#include <QtCore/QDir>
 
 #include <cv_bridge/cv_bridge.h>
 #include <std_srvs/Empty.h>
 #include <std_msgs/Empty.h>
 
 #include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
 
 #include <opencv2/highgui/highgui.hpp>
 
@@ -20,18 +22,29 @@
 #include <rtabmap/core/RtabmapEvent.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/ParamEvent.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/util3d.h>
+
+#include "rtabmap/MsgConversion.h"
 
 #include "PreferencesDialogROS.h"
 
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <laser_geometry/laser_geometry.h>
+
 using namespace rtabmap;
 
-GuiWrapper::GuiWrapper(int & argc, char** argv)
+GuiWrapper::GuiWrapper(int & argc, char** argv) :
+		app_(0),
+		mainWindow_(0),
+		frameId_("base_link"),
+		cameraNodeName_("")
 {
 	ros::NodeHandle nh;
-	infoExTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoExReceivedCallback, this);
 	app_ = new QApplication(argc, argv);
 
-	QString configFile;
+	QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
 	for(int i=1; i<argc; ++i)
 	{
 		if(strcmp(argv[i], "-d") == 0)
@@ -45,21 +58,36 @@ GuiWrapper::GuiWrapper(int & argc, char** argv)
 		}
 	}
 
+	configFile.replace('~', QDir::homePath());
+
+	ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
+	uSleep(500);
 	mainWindow_ = new MainWindow(new PreferencesDialogROS(configFile));
+	mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
 	mainWindow_->show();
-	mainWindow_->changeState(MainWindow::kMonitoring);
+	bool paused = false;
+	nh.param("is_rtabmap_paused", paused, paused);
+	mainWindow_->setMonitoringState(paused);
 	app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );
 
-	resetMemoryClient_ = nh.serviceClient<std_srvs::Empty>("rtabmap/resetMemory");
-	dumpMemoryClient_ = nh.serviceClient<std_srvs::Empty>("rtabmap/dumpMemory");
-	dumpPredictionClient_ = nh.serviceClient<std_srvs::Empty>("rtabmap/dumpPrediction");
-	deleteMemoryClient_ = nh.serviceClient<std_srvs::Empty>("rtabmap/deleteMemory");
+	ros::NodeHandle pnh("~");
 
-	nh = ros::NodeHandle("~");
-	parametersUpdatedPub_ = nh.advertise<std_msgs::Empty>("parameters_updated", 1);
+	// To receive odometry events
+	bool subscribeLaserScan = false;
+	bool subscribeDepth = false;
+	int queueSize = 10;
+	pnh.param("frame_id", frameId_, frameId_);
+	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
+	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+	pnh.param("queue_size", queueSize, queueSize);
+	pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap/camera when pausing the process
+	this->setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
 
 	UEventsManager::addHandler(this);
 	UEventsManager::addHandler(mainWindow_);
+
+	infoExTopic_ = nh.subscribe("infoEx", 1, &GuiWrapper::infoExReceivedCallback, this);
+	mapDataTopic_ = nh.subscribe("mapData", 1, &GuiWrapper::mapDataReceivedCallback, this);
 }
 
 GuiWrapper::~GuiWrapper()
@@ -75,7 +103,7 @@ int GuiWrapper::exec()
 
 void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 {
-	//ROS_INFO("RTAB-Map info ex received!");
+	//ROS_INFO("rtabmapviz: RTAB-Map info ex received!");
 
 	// Map from ROS struct to rtabmap struct
 	rtabmap::Statistics stat;
@@ -83,31 +111,14 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 	stat.setExtended(true); // Extended
 
 	stat.setRefImageId(msg->refId);
+	stat.setRefImageMapId(msg->refMapId);
 	stat.setLoopClosureId(msg->loopClosureId);
+	stat.setLoopClosureMapId(msg->loopClosureMapId);
 	stat.setLocalLoopClosureId(msg->localLoopClosureId);
+	stat.setLocalLoopClosureMapId(msg->localLoopClosureMapId);
 
-	if(msg->refImage.size())
-	{
-		cv::Mat image;
-#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
-		image = cv::imdecode(msg->refImage, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(msg->refImage, -1);
-#endif
-		stat.setRefImage(image);
-		//stat.setRefImage(cv_bridge::toCvShare(msg->refImage, msg)->image.clone());
-	}
-	if(msg->loopImage.size())
-	{
-		cv::Mat image;
-#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
-		image = cv::imdecode(msg->loopImage, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(msg->loopImage, -1);
-#endif
-		stat.setLoopImage(image);
-		//stat.setLoopImage(cv_bridge::toCvShare(msg->loopImage, msg)->image.clone());
-	}
+	stat.setRefImage(msg->refImage);
+	stat.setLoopImage(msg->loopImage);
 
 	//Posterior, likelihood, childCount
 	std::map<int, float> mapIntFloat;
@@ -167,8 +178,107 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 		stat.addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
 	}
 
-	//ROS_INFO("Publishing statistics...");
-	UEventsManager::post(new rtabmap::RtabmapEvent(stat));
+	//RGB-D SLAM data
+	stat.setRefDepth(msg->refDepth);
+	stat.setRefDepth2D(msg->refDepth2D);
+	stat.setLoopDepth(msg->loopDepth);
+	stat.setLoopDepth2D(msg->loopDepth2D);
+
+	stat.setRefDepthConstant(msg->refDepthConstant);
+	stat.setLoopDepthConstant(msg->loopDepthConstant);
+
+	stat.setRefLocalTransform(transformFromGeometryMsg(msg->refLocalTransform));
+	stat.setLoopLocalTransform(transformFromGeometryMsg(msg->loopLocalTransform));
+
+	stat.setMapCorrection(transformFromGeometryMsg(msg->mapCorrection));
+	stat.setLoopClosureTransform(transformFromGeometryMsg(msg->loopClosureTransform));
+	stat.setCurrentPose(transformFromPoseMsg(msg->currentPose));
+
+	std::map<int, Transform> poses;
+	for(unsigned int i=0; i<msg->nodeIds.size() && i<msg->nodePoses.size(); ++i)
+	{
+		poses.insert(std::make_pair(msg->nodeIds[i], transformFromPoseMsg(msg->nodePoses[i])));
+	}
+	stat.setPoses(poses);
+
+	this->post(new RtabmapEvent(stat));
+}
+
+void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
+{
+	std::map<int, std::vector<unsigned char> > images;
+	std::map<int, std::vector<unsigned char> > depths;
+	std::map<int, std::vector<unsigned char> > depths2d;
+	std::map<int, float> depthConstants;
+	std::map<int, Transform> localTransforms;
+	std::map<int, Transform> poses;
+	Transform mapCorrection;
+
+	if(msg->imageIDs.size() != msg->images.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... images and IDs are not the same size (%d vs %d)!",
+				(int)msg->images.size(), (int)msg->imageIDs.size());
+	}
+
+	if(msg->depthIDs.size() != msg->depths.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depths and IDs are not the same size (%d vs %d)!",
+				(int)msg->depths.size(), (int)msg->depthIDs.size());
+	}
+
+	if(msg->depth2DIDs.size() != msg->depths2D.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depths2D and IDs are not the same size (%d vs %d)!",
+				(int)msg->depths2D.size(), (int)msg->depth2DIDs.size());
+	}
+
+	if(msg->depthConstantIDs.size() != msg->depthConstants.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depthConstants and IDs are not the same size (%d vs %d)!",
+				(int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size());
+	}
+
+	for(unsigned int i=0; i<msg->imageIDs.size() && i < msg->images.size(); ++i)
+	{
+		images.insert(std::make_pair(msg->imageIDs[i], msg->images[i].bytes));
+	}
+
+	for(unsigned int i=0; i<msg->depthIDs.size() && i < msg->depths.size(); ++i)
+	{
+		depths.insert(std::make_pair(msg->depthIDs[i], msg->depths[i].bytes));
+	}
+
+	for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depths2D.size(); ++i)
+	{
+		depths2d.insert(std::make_pair(msg->depth2DIDs[i], msg->depths2D[i].bytes));
+	}
+
+	for(unsigned int i=0; i<msg->depthConstantIDs.size() && i < msg->depthConstants.size(); ++i)
+	{
+		depthConstants.insert(std::make_pair(msg->depthConstantIDs[i], msg->depthConstants[i]));
+	}
+
+	for(unsigned int i=0; i<msg->localTransformIDs.size() && i < msg->localTransforms.size(); ++i)
+	{
+		Transform t = transformFromGeometryMsg(msg->localTransforms[i]);
+		localTransforms.insert(std::make_pair(msg->localTransformIDs[i], t));
+	}
+
+	for(unsigned int i=0; i<msg->poseIDs.size() && i < msg->poses.size(); ++i)
+	{
+		Transform t = transformFromPoseMsg(msg->poses[i]);
+		poses.insert(std::make_pair(msg->poseIDs[i], t));
+	}
+
+	mapCorrection = transformFromGeometryMsg(msg->mapCorrection);
+
+	this->post(new RtabmapEvent3DMap(images,
+			depths,
+			depths2d,
+			depthConstants,
+			localTransforms,
+			poses,
+			mapCorrection));
 }
 
 void GuiWrapper::handleEvent(UEvent * anEvent)
@@ -178,7 +288,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 		const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
 		rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
 		bool modified = false;
-		ros::NodeHandle nh("rtabmap");
+		ros::NodeHandle nh;
 		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
 		{
 			//save only parameters with valid names
@@ -195,44 +305,280 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 		if(modified)
 		{
 			ROS_INFO("Parameters updated");
-			parametersUpdatedPub_.publish(std_msgs::Empty());
+			std_srvs::Empty srv;
+			if(!ros::service::call("update_parameters", srv))
+			{
+				ROS_ERROR("Can't call \"update_parameters\" service");
+			}
 		}
 	}
 	else if(anEvent->getClassName().compare("RtabmapEventCmd") == 0)
 	{
 		std_srvs::Empty srv;
-		rtabmap::RtabmapEventCmd::Cmd cmd = ((rtabmap::RtabmapEventCmd *)anEvent)->getCmd();
-		if(cmd == rtabmap::RtabmapEventCmd::kCmdDumpMemory)
+		rtabmap::RtabmapEventCmd * cmdEvent = (rtabmap::RtabmapEventCmd *)anEvent;
+		rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
+		if(cmd == rtabmap::RtabmapEventCmd::kCmdDeleteMemory)
 		{
-			if(!dumpMemoryClient_.call(srv))
+			if(!ros::service::call("reset", srv))
 			{
-				ROS_ERROR("Can't call \"dumpMemory\" service");
+				ROS_ERROR("Can't call \"reset\" service");
 			}
 		}
-		else if(cmd == rtabmap::RtabmapEventCmd::kCmdDumpPrediction)
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPause)
 		{
-			if(!dumpPredictionClient_.call(srv))
+			if(cmdEvent->getInt())
 			{
-				ROS_ERROR("Can't call \"dumpPrediction\" service");
+				// Pause the camera if the rtabmap/camera node is used
+				if(!cameraNodeName_.empty())
+				{
+					std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str());
+					system(str.c_str());
+				}
+
+				// Pause rtabmap
+				if(!ros::service::call("pause", srv))
+				{
+					ROS_ERROR("Can't call \"pause\" service");
+				}
+			}
+			else
+			{
+				// Resume rtabmap
+				if(!ros::service::call("resume", srv))
+				{
+					ROS_ERROR("Can't call \"resume\" service");
+				}
+
+				// Resume the camera if the rtabmap/camera node is used
+				if(!cameraNodeName_.empty())
+				{
+					std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str());
+					system(str.c_str());
+				}
 			}
 		}
-		else if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
 		{
-			if(!resetMemoryClient_.call(srv))
+			if(!ros::service::call("trigger_new_map", srv))
 			{
-				ROS_ERROR("Can't call \"resetMemory\" service");
+				ROS_ERROR("Can't call \"trigger_new_map\" service");
 			}
 		}
-		else if(cmd == rtabmap::RtabmapEventCmd::kCmdDeleteMemory)
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMap)
 		{
-			if(!deleteMemoryClient_.call(srv))
+			if(mapDataTopic_.getNumPublishers())
 			{
-				ROS_ERROR("Can't call \"deleteMemory\" service");
+				if(!ros::service::call("publish_map_data", srv))
+				{
+					ROS_WARN("Can't call \"publish_map_data\" service");
+					this->post(new RtabmapEvent3DMap(1)); // service error
+				}
+			}
+			else
+			{
+				ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str());
+				this->post(new RtabmapEvent3DMap(2)); // topic error
 			}
 		}
 		else
 		{
-			ROS_WARN("Unknown command...");
+			ROS_WARN("Not handled command (%d)...", cmd);
+		}
+	}
+	else if(anEvent->getClassName().compare("OdometryResetEvent") == 0)
+	{
+		std_srvs::Empty srv;
+		if(!ros::service::call("reset_odom", srv))
+		{
+			ROS_ERROR("Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)");
 		}
 	}
 }
+
+void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
+{
+	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+	rtabmap::Image image(
+			cv::Mat(),
+			cv::Mat(),
+			0.0f,
+			odom,
+			Transform());
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::depthCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	// TF ready?
+	Transform localTransform;
+	try
+	{
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+		localTransform = transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+		return;
+	}
+
+	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+	float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+	rtabmap::Image image(
+			ptrImage->image.clone(),
+			ptrDepth->image.clone(),
+			depthConstant,
+			odom,
+			localTransform);
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::scanCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	// TF ready?
+	try
+	{
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+		return;
+	}
+
+	//transform in frameId_ frame
+	sensor_msgs::PointCloud2 scanOut;
+	laser_geometry::LaserProjection projection;
+	projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+	pcl::PointCloud<pcl::PointXYZ> pclScan;
+	pcl::fromROSMsg(scanOut, pclScan);
+	cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
+
+	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+
+	rtabmap::Image image(
+			ptrImage->image.clone(),
+			cv::Mat(),
+			scan,
+			0.0f,
+			odom,
+			Transform());
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::depthScanCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	// TF ready?
+	Transform localTransform;
+	try
+	{
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
+		tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+		localTransform = transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+		return;
+	}
+
+	//transform in frameId_ frame
+	sensor_msgs::PointCloud2 scanOut;
+	laser_geometry::LaserProjection projection;
+	projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+	pcl::PointCloud<pcl::PointXYZ> pclScan;
+	pcl::fromROSMsg(scanOut, pclScan);
+	cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
+
+	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+	float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+	rtabmap::Image image(
+			ptrImage->image.clone(),
+			ptrDepth->image.clone(),
+			scan,
+			depthConstant,
+			odom,
+			localTransform);
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::setupCallbacks(
+		bool subscribeDepth,
+		bool subscribeLaserScan,
+		int queueSize)
+{
+	ros::NodeHandle nh; // public
+	ros::NodeHandle pnh("~"); // private
+	ros::NodeHandle rgb_nh(nh, "rgb");
+	ros::NodeHandle depth_nh(nh, "depth");
+	ros::NodeHandle rgb_pnh(pnh, "rgb");
+	ros::NodeHandle depth_pnh(pnh, "depth");
+	image_transport::ImageTransport rgb_it(rgb_nh);
+	image_transport::ImageTransport depth_it(depth_nh);
+	image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+	image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+	if(subscribeDepth && subscribeLaserScan)
+	{
+		ROS_INFO("Registering Depth+LaserScan callback...");
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+		odomSub_.subscribe(nh, "odom", 1);
+		scanSub_.subscribe(nh, "scan", 1);
+		depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+	}
+	else if(subscribeDepth && !subscribeLaserScan)
+	{
+		ROS_INFO("Registering Depth callback...");
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+		odomSub_.subscribe(nh, "odom", 1);
+		depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
+		depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
+	}
+	else if(!subscribeDepth && subscribeLaserScan)
+	{
+		ROS_INFO("Registering LaserScan callback...");
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		odomSub_.subscribe(nh, "odom", 1);
+		scanSub_.subscribe(nh, "scan", 1);
+		scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
+		scanSync_->registerCallback(boost::bind(&GuiWrapper::scanCallback, this, _1, _2, _3));
+	}
+	else // default odom only
+	{
+		ROS_INFO("Registering default callback...");
+		defaultSub_ = nh.subscribe("odom", 1, &GuiWrapper::defaultCallback, this);
+	}
+}
+

rtabmap/src/GuiWrapper.h

@@ -10,8 +10,24 @@
 
 #include <ros/ros.h>
 #include "rtabmap/InfoEx.h"
+#include "rtabmap/MapData.h"
 #include "rtabmap/utilite/UEventsHandler.h"
+
+#include <tf/transform_listener.h>
+
 #include <geometry_msgs/TwistStamped.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/LaserScan.h>
+#include <nav_msgs/Odometry.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
 
 namespace rtabmap
 {
@@ -32,20 +48,62 @@ protected:
 	virtual void handleEvent(UEvent * anEvent);
 
 private:
-	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & infoMsg);
+	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg);
+	void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg);
+
+	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize);
+	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom
+	void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					   const nav_msgs::OdometryConstPtr & odomMsg,
+					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void scanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					  const nav_msgs::OdometryConstPtr & odomMsg,
+					  const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+						   const nav_msgs::OdometryConstPtr & odomMsg,
+						   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+						   const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
+						   const sensor_msgs::LaserScanConstPtr& scanMsg);
 
 private:
 	ros::Subscriber infoExTopic_;
+	ros::Subscriber mapDataTopic_;
 	QApplication * app_;
 	rtabmap::MainWindow * mainWindow_;
+	std::string cameraNodeName_;
 
-	ros::ServiceClient resetMemoryClient_;
-	ros::ServiceClient dumpMemoryClient_;
-	ros::ServiceClient changeCameraImgRateClient_;
-	ros::ServiceClient deleteMemoryClient_;
-	ros::ServiceClient dumpPredictionClient_;
+	// odometry subscription stuffs
+	std::string frameId_;
+	tf::TransformListener tfListener_;
 
-	ros::Publisher parametersUpdatedPub_;
+	ros::Subscriber defaultSub_; // odometry only
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
+	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
+	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
+	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::LaserScan> MyScanSyncPolicy;
+	message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
 };
 
 #endif /* GUIWRAPPER_H_ */

rtabmap/src/MapAssemblerNode.cpp

@@ -0,0 +1,278 @@
+/*
+ * GuiWrapper.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include <QtGui/QApplication>
+#include <QtCore/QDir>
+
+#include <cv_bridge/cv_bridge.h>
+#include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <rtabmap/gui/MainWindow.h>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/ParamEvent.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/util3d.h>
+
+#include "rtabmap/MsgConversion.h"
+
+#include "PreferencesDialogROS.h"
+
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <laser_geometry/laser_geometry.h>
+
+using namespace rtabmap;
+
+class MapAssembler
+{
+
+public:
+	MapAssembler() :
+		cloudDecimation_(4),
+		cloudVoxelSize_(0.02),
+		scanVoxelSize_(0.01)
+	{
+		ros::NodeHandle pnh("~");
+		pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
+		pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
+		pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+
+		ros::NodeHandle nh;
+		infoExTopic_ = nh.subscribe("infoEx", 1, &MapAssembler::infoExReceivedCallback, this);
+		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
+
+		assembledMapClouds_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_clouds", 1);
+		assembledMapScans_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_scans", 1);
+	}
+
+	~MapAssembler()
+	{
+	}
+
+	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
+	{
+		if(!uContains(rgbClouds_, msg->refId) && msg->refImage.size() && msg->refDepth.size() && msg->refDepthConstant > 0)
+		{
+			rtabmap::Transform localTransform = transformFromGeometryMsg(msg->refLocalTransform);
+
+			if(!localTransform.isNull())
+			{
+				cv::Mat image = util3d::uncompressImage(msg->refImage);
+				cv::Mat depth = util3d::uncompressImage(msg->refDepth);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, msg->refDepthConstant, cloudDecimation_);
+
+				if(cloudVoxelSize_ > 0)
+				{
+					cloud = util3d::voxelize(cloud, cloudVoxelSize_);
+				}
+
+				cloud = util3d::transformPointCloud(cloud, localTransform);
+
+				rgbClouds_.insert(std::make_pair(msg->refId, cloud));
+			}
+		}
+
+		if(!uContains(scans_, msg->refId) && msg->refDepth2D.size())
+		{
+			cv::Mat depth2d = util3d::uncompressData(msg->refDepth2D);
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::depth2DToPointCloud(depth2d);
+			if(scanVoxelSize_ > 0)
+			{
+				cloud = util3d::voxelize(cloud, scanVoxelSize_);
+			}
+
+			scans_.insert(std::make_pair(msg->refId, cloud));
+		}
+
+		if(assembledMapClouds_.getNumSubscribers())
+		{
+			// generate the assembled cloud!
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+			for(unsigned int i=0; i<msg->nodeIds.size() && i<msg->nodePoses.size(); ++i)
+			{
+				Transform pose = transformFromPoseMsg(msg->nodePoses[i]);
+
+				std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter = rgbClouds_.find(msg->nodeIds[i]);
+				if(iter != rgbClouds_.end())
+				{
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(iter->second, pose);
+					*assembledCloud+=*transformed;
+				}
+			}
+
+			if(assembledCloud->size())
+			{
+				if(cloudVoxelSize_ > 0)
+				{
+					assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_);
+				}
+
+				sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+				pcl::toROSMsg(*assembledCloud, *cloudMsg);
+				cloudMsg->header.stamp = ros::Time::now();
+				cloudMsg->header.frame_id = msg->header.frame_id;
+				assembledMapClouds_.publish(cloudMsg);
+			}
+		}
+
+		if(assembledMapScans_.getNumSubscribers())
+		{
+			// generate the assembled scan!
+			pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+			for(unsigned int i=0; i<msg->nodeIds.size() && i<msg->nodePoses.size(); ++i)
+			{
+				Transform pose = transformFromPoseMsg(msg->nodePoses[i]);
+
+				std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = scans_.find(msg->nodeIds[i]);
+				if(iter != scans_.end())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(iter->second, pose);
+					*assembledCloud+=*transformed;
+				}
+			}
+
+			if(assembledCloud->size())
+			{
+				if(scanVoxelSize_ > 0)
+				{
+					assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
+				}
+
+				sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+				pcl::toROSMsg(*assembledCloud, *cloudMsg);
+				cloudMsg->header.stamp = ros::Time::now();
+				cloudMsg->header.frame_id = msg->header.frame_id;
+				assembledMapScans_.publish(cloudMsg);
+			}
+		}
+	}
+
+	void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
+	{
+		std::map<int, std::vector<unsigned char> > images;
+		std::map<int, std::vector<unsigned char> > depths;
+		std::map<int, std::vector<unsigned char> > depths2d;
+		std::map<int, float> depthConstants;
+		std::map<int, Transform> localTransforms;
+		std::map<int, Transform> poses;
+
+		if(msg->imageIDs.size() != msg->images.size())
+		{
+			ROS_WARN("rtabmapviz: receiving map... images and IDs are not the same size (%d vs %d)!",
+					(int)msg->images.size(), (int)msg->imageIDs.size());
+		}
+
+		if(msg->depthIDs.size() != msg->depths.size())
+		{
+			ROS_WARN("rtabmapviz: receiving map... depths and IDs are not the same size (%d vs %d)!",
+					(int)msg->depths.size(), (int)msg->depthIDs.size());
+		}
+
+		if(msg->depthConstantIDs.size() != msg->depthConstants.size())
+		{
+			ROS_WARN("rtabmapviz: receiving map... depthConstants and IDs are not the same size (%d vs %d)!",
+					(int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size());
+		}
+
+		if(msg->depth2DIDs.size() != msg->depths2D.size())
+		{
+			ROS_WARN("rtabmapviz: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!",
+					(int)msg->depths2D.size(), (int)msg->depth2DIDs.size());
+		}
+
+		// fill maps
+		for(unsigned int i=0; i<msg->imageIDs.size() && i < msg->images.size(); ++i)
+		{
+			if(!uContains(rgbClouds_, msg->imageIDs[i]))
+			{
+				images.insert(std::make_pair(msg->imageIDs[i], msg->images[i].bytes));
+			}
+		}
+
+		for(unsigned int i=0; i<msg->depthIDs.size() && i < msg->depths.size(); ++i)
+		{
+			if(!uContains(rgbClouds_, msg->depthIDs[i]))
+			{
+				depths.insert(std::make_pair(msg->depthIDs[i], msg->depths[i].bytes));
+			}
+		}
+
+		for(unsigned int i=0; i<msg->depthConstantIDs.size() && i < msg->depthConstants.size(); ++i)
+		{
+			if(!uContains(rgbClouds_, msg->depthConstantIDs[i]))
+			{
+				depthConstants.insert(std::make_pair(msg->depthConstantIDs[i], msg->depthConstants[i]));
+			}
+		}
+
+		for(unsigned int i=0; i<msg->localTransformIDs.size() && i < msg->localTransforms.size(); ++i)
+		{
+			if(!uContains(rgbClouds_, msg->localTransformIDs[i]))
+			{
+				Transform t = transformFromGeometryMsg(msg->localTransforms[i]);
+				localTransforms.insert(std::make_pair(msg->localTransformIDs[i], t));
+			}
+		}
+
+		for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depths2D.size(); ++i)
+		{
+			if(!uContains(scans_, msg->depth2DIDs[i]))
+			{
+				cv::Mat depth2d = util3d::uncompressData(msg->depths2D[i].bytes);
+				scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d)));
+			}
+		}
+
+		// create clouds
+		for(std::map<int, std::vector<unsigned char> >::iterator iter = images.begin(); iter!=images.end(); ++iter)
+		{
+			if(uContains(depths, iter->first) && uContains(depthConstants, iter->first) && uContains(localTransforms, iter->first))
+			{
+				cv::Mat image = util3d::uncompressImage(iter->second);
+				cv::Mat depth = util3d::uncompressImage(depths.at(iter->first));
+				float depthConstant = depthConstants.at(iter->first);
+				rtabmap::Transform localTransform = localTransforms.at(iter->first);
+				rgbClouds_.insert(std::make_pair(iter->first, util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_)));
+			}
+		}
+	}
+
+private:
+	int cloudDecimation_;
+	double cloudVoxelSize_;
+	double scanVoxelSize_;
+
+	ros::Subscriber infoExTopic_;
+	ros::Subscriber mapDataTopic_;
+
+	ros::Publisher assembledMapClouds_;
+	ros::Publisher assembledMapScans_;
+
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > rgbClouds_;
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "map_assembler");
+	MapAssembler assembler;
+	ros::spin();
+	return 0;
+}

rtabmap/src/MsgConversion.cpp

@@ -0,0 +1,74 @@
+
+#include "rtabmap/MsgConversion.h"
+
+#include <opencv2/highgui/highgui.hpp>
+#include <zlib.h>
+#include <ros/ros.h>
+#include <rtabmap/core/util3d.h>
+#include <tf_conversions/tf_eigen.h>
+
+namespace rtabmap {
+
+void transformToTF(const rtabmap::Transform & transform, tf::Transform & tfTransform)
+{
+	if(!transform.isNull())
+	{
+		tf::transformEigenToTF(util3d::transformToEigen3d(transform), tfTransform);
+	}
+	else
+	{
+		tfTransform = tf::Transform();
+	}
+}
+
+rtabmap::Transform transformFromTF(const tf::Transform & transform)
+{
+	Eigen::Affine3d eigenTf;
+	tf::transformTFToEigen(transform, eigenTf);
+	return util3d::transformFromEigen3d(eigenTf);
+}
+
+void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::Transform & msg)
+{
+	if(!transform.isNull())
+	{
+		tf::Transform tfTransform;
+		transformToTF(transform, tfTransform);
+		tf::transformTFToMsg(tfTransform, msg);
+	}
+	else
+	{
+		msg = geometry_msgs::Transform();
+	}
+}
+
+
+rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg)
+{
+	tf::Transform tfTransform;
+	tf::transformMsgToTF(msg, tfTransform);
+	return transformFromTF(tfTransform);
+}
+
+void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg)
+{
+	if(!transform.isNull())
+	{
+		tf::Transform tfTransform;
+		transformToTF(transform, tfTransform);
+		tf::poseTFToMsg(tfTransform, msg);
+	}
+	else
+	{
+		msg = geometry_msgs::Pose();
+	}
+}
+
+rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg)
+{
+	tf::Pose tfTransform;
+	tf::poseMsgToTF(msg, tfTransform);
+	return transformFromTF(tfTransform);
+}
+
+}

rtabmap/src/PreferencesDialogROS.cpp

@@ -26,10 +26,10 @@ PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) :
 
 PreferencesDialogROS::~PreferencesDialogROS()
 {
-
+	ROS_INFO("rtabmapviz: GUI settings are saved to \"%s\"", configFile_.toStdString().c_str());
 }
 
-QString PreferencesDialogROS::getIniFilePath()
+QString PreferencesDialogROS::getIniFilePath() const
 {
 	if(configFile_.isEmpty())
 	{
@@ -52,7 +52,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 {
 	if(filePath.isEmpty())
 	{
-		ros::NodeHandle nh("rtabmap");
+		ros::NodeHandle nh;
 		ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
 		bool validParameters = true;
 		int readCount = 0;

rtabmap/src/PreferencesDialogROS.h

@@ -19,7 +19,7 @@ public:
 	PreferencesDialogROS(const QString & configFile);
 	virtual ~PreferencesDialogROS();
 
-	virtual QString getIniFilePath();
+	virtual QString getIniFilePath() const;
 
 protected:
 	virtual QString getParamMessage();

rtabmap/src/VisualOdometryNode.cpp

@@ -0,0 +1,310 @@
+/*
+ * visualodometry_.cpp
+ *
+ *  Created on: 2013-01-16
+ *      Author: mathieu
+ */
+
+#include <ros/ros.h>
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+#include <std_srvs/Empty.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <nav_msgs/Odometry.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/util3d.h>
+#include "rtabmap/MsgConversion.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UStl.h"
+
+using namespace rtabmap;
+
+class VisualOdometry
+{
+public:
+	VisualOdometry() :
+		odometry_(0),
+		frameId_("base_link"),
+		odomFrameId_("odom"),
+		sync_(0)
+	{
+		ros::NodeHandle nh;
+
+		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
+
+		ros::NodeHandle pnh("~");
+
+		int queueSize = 5;
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+		pnh.param("queue_size", queueSize, queueSize);
+
+		//parameters
+		rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+		rtabmap::ParametersMap parametersOdom;
+		for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			if((uSplit(iter->first, '/').front().compare("Odom") == 0 ||
+				uSplit(iter->first, '/').front().compare("OdomBin") == 0 ||
+				uSplit(iter->first, '/').front().compare("Kp") == 0)
+			   &&
+			   uSplit(iter->first, '/').front().compare("OdomICP") != 0)
+			{
+				parametersOdom.insert(*iter);
+			}
+		}
+
+		for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+		{
+			std::string vStr;
+			bool vBool;
+			int vInt;
+			double vDouble;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+				iter->second = vStr;
+			}
+			else if(pnh.getParam(iter->first, vBool))
+			{
+				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+				iter->second = uBool2Str(vBool);
+			}
+			else if(pnh.getParam(iter->first, vInt))
+			{
+				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+				iter->second = uNumber2Str(vInt);
+
+				if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
+				{
+					ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
+					iter->second = uNumber2Str(8);
+				}
+			}
+			else if(pnh.getParam(iter->first, vDouble))
+			{
+				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+				iter->second = uNumber2Str(vDouble);
+			}
+		}
+
+		if(std::atoi(parametersOdom.at(Parameters::kOdomType()).c_str()) == 1)
+		{
+			odometry_ = new rtabmap::OdometryBinary(parametersOdom);
+		}
+		else
+		{
+			odometry_ = new rtabmap::OdometryBOW(parametersOdom);
+		}
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info", 1);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
+		sync_->registerCallback(boost::bind(&VisualOdometry::callback, this, _1, _2, _3));
+
+		resetSrv_ = nh.advertiseService("reset_odom", &VisualOdometry::reset, this);
+	}
+
+	~VisualOdometry()
+	{
+		delete sync_;
+		delete odometry_;
+	}
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+		   !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
+		     depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0))
+		{
+			ROS_ERROR("Input type must be image=mono8,rgb8,bgr8 and image_depth=16UC1");
+			return;
+		}
+		else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
+		{
+			static bool warned = false;
+			if(!warned)
+			{
+				ROS_WARN("Input depth type is 32FC1, please use type 16UC1 for depth. The depth images "
+						 "will be processed anyway but with a conversion. This warning is only be printed once...");
+				warned = true;
+			}
+		}
+
+		tf::StampedTransform localTransform;
+		try
+		{
+			tfListener_.lookupTransform(frameId_, image->header.frame_id, image->header.stamp, localTransform);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		ros::WallTime time = ros::WallTime::now();
+
+		if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
+		{
+			float depthConstant = 1.0f/cameraInfo->K[4];
+			cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image);
+			cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
+
+			rtabmap::Image data(ptrImage->image,
+					ptrDepth->image.type() == CV_32FC1?util3d::cvtDepthFromFloat(ptrDepth->image):ptrDepth->image,
+					depthConstant,
+					rtabmap::Transform(),
+					rtabmap::transformFromTF(localTransform));
+			rtabmap::Transform pose = odometry_->process(data);
+			if(!pose.isNull())
+			{
+				//*********************
+				// Update odometry
+				//*********************
+				tf::Transform poseTF;
+				rtabmap::transformToTF(pose, poseTF);
+
+				tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, image->header.stamp, odomFrameId_, frameId_));
+
+				if(odomPub_.getNumSubscribers())
+				{
+					//next, we'll publish the odometry message over ROS
+					nav_msgs::Odometry odom;
+					odom.header.stamp = image->header.stamp; // use corresponding time stamp to image
+					odom.header.frame_id = odomFrameId_;
+					odom.child_frame_id = frameId_;
+
+					//set the position
+					odom.pose.pose.position.x = poseTF.getOrigin().x();
+					odom.pose.pose.position.y = poseTF.getOrigin().y();
+					odom.pose.pose.position.z = poseTF.getOrigin().z();
+					tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
+
+					//publish the message
+					odomPub_.publish(odom);
+				}
+			}
+			else
+			{
+				//ROS_WARN("Odometry lost!");
+
+				//send null pose to notify that odometry is lost
+				nav_msgs::Odometry odom;
+				odom.header.stamp = image->header.stamp; // use corresponding time stamp to image
+				odom.header.frame_id = odomFrameId_;
+				odom.child_frame_id = frameId_;
+
+				//publish the message
+				odomPub_.publish(odom);
+			}
+		}
+
+		ROS_INFO("Odom update time(%f s)", (ros::WallTime::now()-time).toSec());
+	}
+
+	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("visual_odometry: reset odom!");
+		odometry_->reset();
+		return true;
+	}
+
+private:
+	rtabmap::Odometry * odometry_;
+
+	// parameters
+	std::string frameId_;
+	std::string odomFrameId_;
+
+	ros::Publisher odomPub_;
+	ros::ServiceServer resetSrv_;
+	tf::TransformBroadcaster tfBroadcaster_;
+	tf::TransformListener tfListener_;
+
+	image_transport::SubscriberFilter image_mono_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+};
+
+int main(int argc, char *argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "visual_odometry");
+
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+			rtabmap::ParametersMap parametersOdom;
+			if(strcmp(argv[i], "--params") == 0)
+			{
+				// show specific parameters
+				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+				{
+					if((uSplit(iter->first, '/').front().compare("Odom") == 0 ||
+						uSplit(iter->first, '/').front().compare("OdomBin") == 0 ||
+						uSplit(iter->first, '/').front().compare("Kp") == 0)
+					   &&
+					   uSplit(iter->first, '/').front().compare("OdomICP") != 0)
+					{
+						parametersOdom.insert(*iter);
+					}
+				}
+			}
+
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+	}
+
+	VisualOdometry vOdom;
+	ros::spin();
+	return 0;
+}

rtabmap/src/nodelets/data_odom_sync.cpp

@@ -0,0 +1,104 @@
+
+#include "ros/ros.h"
+#include "pluginlib/class_list_macros.h"
+#include "nodelet/nodelet.h"
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <sensor_msgs/CameraInfo.h>
+#include <nav_msgs/Odometry.h>
+
+namespace rtabmap
+{
+
+class DataOdomSyncNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	DataOdomSyncNodelet():
+		sync_(0)
+	{
+	}
+
+	virtual ~DataOdomSyncNodelet()
+	{
+		delete sync_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(private_nh, "rgb");
+		ros::NodeHandle depth_pnh(private_nh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		int queueSize = 10;
+		private_nh.param("queue_size", queueSize, queueSize);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_, odom_sub_);
+		sync_->registerCallback(boost::bind(&DataOdomSyncNodelet::callback, this, _1, _2, _3, _4));
+
+		image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
+		odom_sub_.subscribe(nh, "odom_in", 1);
+
+		imagePub_ = rgb_it.advertise("image_out", 10);
+		imageDepthPub_ = depth_it.advertise("image_out", 10);
+		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
+		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom_out", 10);
+	};
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& imageDepth,
+			const sensor_msgs::CameraInfoConstPtr& camInfo,
+			const nav_msgs::OdometryConstPtr & odom)
+	{
+		if(imagePub_.getNumSubscribers())
+		{
+			imagePub_.publish(image);
+		}
+		if(imageDepthPub_.getNumSubscribers())
+		{
+			imageDepthPub_.publish(imageDepth);
+		}
+		if(infoPub_.getNumSubscribers())
+		{
+			infoPub_.publish(camInfo);
+		}
+		if(odomPub_.getNumSubscribers())
+		{
+			odomPub_.publish(odom);
+		}
+	}
+
+	image_transport::Publisher imagePub_;
+	image_transport::Publisher imageDepthPub_;
+	ros::Publisher infoPub_;
+	ros::Publisher odomPub_;
+
+	image_transport::SubscriberFilter image_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+
+};
+
+
+PLUGINLIB_DECLARE_CLASS(rtabmap, data_odom_sync, rtabmap::DataOdomSyncNodelet, nodelet::Nodelet);
+}

rtabmap/src/nodelets/data_throttle.cpp

@@ -0,0 +1,111 @@
+#include "ros/ros.h"
+#include "pluginlib/class_list_macros.h"
+#include "nodelet/nodelet.h"
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <sensor_msgs/CameraInfo.h>
+
+namespace rtabmap
+{
+
+class DataThrottleNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	DataThrottleNodelet():
+		max_update_rate_(0),
+		sync_(0)
+	{
+	}
+
+	virtual ~DataThrottleNodelet()
+	{
+		delete sync_;
+	}
+
+private:
+	ros::Time last_update_;
+	double max_update_rate_;
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(private_nh, "rgb");
+		ros::NodeHandle depth_pnh(private_nh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		int queueSize = 10;
+		private_nh.param("max_rate", max_update_rate_, max_update_rate_);
+		private_nh.param("queue_size", queueSize, queueSize);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
+		sync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
+
+		image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
+
+		imagePub_ = rgb_it.advertise("image_out", 10);
+		imageDepthPub_ = depth_it.advertise("image_out", 10);
+		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
+	};
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& imageDepth,
+			const sensor_msgs::CameraInfoConstPtr& camInfo)
+	{
+		if (max_update_rate_ > 0.0)
+		{
+			NODELET_DEBUG("update set to %f", max_update_rate_);
+			if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
+			{
+				NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
+				return;
+			}
+		}
+		else
+			NODELET_DEBUG("update_rate unset continuing");
+
+		last_update_ = ros::Time::now();
+
+		if(imagePub_.getNumSubscribers())
+		{
+			imagePub_.publish(image);
+		}
+		if(imageDepthPub_.getNumSubscribers())
+		{
+			imageDepthPub_.publish(imageDepth);
+		}
+		if(infoPub_.getNumSubscribers())
+		{
+			infoPub_.publish(camInfo);
+		}
+	}
+
+	image_transport::Publisher imagePub_;
+	image_transport::Publisher imageDepthPub_;
+	ros::Publisher infoPub_;
+
+	image_transport::SubscriberFilter image_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+
+};
+
+
+PLUGINLIB_DECLARE_CLASS(rtabmap, data_throttle, rtabmap::DataThrottleNodelet, nodelet::Nodelet);
+}

rtabmap/src/nodelets/point_cloud_xyzrgb.cpp

@@ -0,0 +1,140 @@
+/*
+ * visual_odometry.cpp
+ *
+ *  Created on: 2013-01-16
+ *      Author: mathieu
+ */
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/util3d.h"
+
+namespace rtabmap
+{
+
+class PointCloudXYZRGB : public nodelet::Nodelet
+{
+public:
+	PointCloudXYZRGB() : voxelSize_(0.0) {}
+
+	virtual ~PointCloudXYZRGB()
+	{
+		delete sync_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("voxel_size", voxelSize_, voxelSize_);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+		sync_->registerCallback(boost::bind(&PointCloudXYZRGB::callback, this, _1, _2, _3));
+		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+	}
+
+
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& image,
+			  const sensor_msgs::ImageConstPtr& imageDepth,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
+			(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
+			 imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
+		{
+			ROS_ERROR("Input type must be image=mono8,rgb8,bgr8 and image_depth=32FC1,16UC1");
+			return;
+		}
+
+
+		cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+		cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+		pclCloud = rtabmap::util3d::cloudFromDepthRGB(
+				imagePtr->image,
+				imageDepthPtr->image,
+				cameraInfo->K[2],
+				cameraInfo->K[5],
+				cameraInfo->K[0],
+				cameraInfo->K[4]);
+
+		if(voxelSize_ > 0.0)
+		{
+			pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
+		}
+
+		//*********************
+		// Publish Map
+		//*********************
+		if(cloudPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*pclCloud, rosCloud);
+			rosCloud.header.stamp = image->header.stamp;
+			rosCloud.header.frame_id = image->header.frame_id;
+
+			//publish the message
+			cloudPub_.publish(rosCloud);
+		}
+}
+
+private:
+
+	double voxelSize_;
+	ros::Publisher cloudPub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+
+	// without odometry subscription (odometry is computed by this node)
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+};
+
+PLUGINLIB_DECLARE_CLASS(rtabmap, point_cloud_xyzrgb, rtabmap::PointCloudXYZRGB, nodelet::Nodelet);
+}
+