switch rtabmap_lib svn co from the 0.3 branch to trunk

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

rtabmap/src/AbtrVelocityNode.cpp

@@ -0,0 +1,94 @@
+/*
+ * CameraNode.cpp
+ *
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <utilite/UMutex.h>
+
+UMutex commandMutex;
+std::vector<float> commandsA;
+std::vector<float> commandsB;
+int commandSize = 0;
+int commandIndex = 0;
+ros::Publisher rosPublisher;
+
+void velocityAReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
+{
+	//ROS_INFO("Received command velocity A (%f,%f)", msg->linear, msg->angular);
+	commandMutex.lock();
+	{
+		commandsA.push_back(msg->linear.x);
+		commandsA.push_back(msg->linear.y);
+		commandsA.push_back(msg->linear.z);
+		commandsA.push_back(msg->angular.x);
+		commandsA.push_back(msg->angular.y);
+		commandsA.push_back(msg->angular.z);
+	}
+	commandMutex.unlock();
+}
+
+void velocityBReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
+{
+	//ROS_INFO("Received command velocity B (%f,%f)", msg->linear, msg->angular);
+	commandMutex.lock();
+	{
+		commandsB.push_back(msg->linear.x);
+		commandsB.push_back(msg->linear.y);
+		commandsB.push_back(msg->linear.z);
+		commandsB.push_back(msg->angular.x);
+		commandsB.push_back(msg->angular.y);
+		commandsB.push_back(msg->angular.z);
+	}
+	commandMutex.unlock();
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "input_node");
+	ros::NodeHandle n;
+	ros::Subscriber velATopic;
+	ros::Subscriber velBTopic;
+	velATopic = n.subscribe("cmd_vel_a", 1, velocityAReceivedCallback);
+	velBTopic = n.subscribe("cmd_vel_b", 1, velocityBReceivedCallback);
+	rosPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+
+	ros::Rate loop_rate(10); // 10 Hz
+	while(ros::ok())
+	{
+		commandMutex.lock();
+		{
+			// priority for commandsA
+			if(commandsA.size())
+			{
+				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
+				vel->linear.x = commandsA[0];
+				vel->linear.y = commandsA[1];
+				vel->linear.z = commandsA[2];
+				vel->angular.x = commandsA[3];
+				vel->angular.y = commandsA[4];
+				vel->angular.z = commandsA[5];
+				rosPublisher.publish(vel);
+			}
+			else if(commandsB.size())
+			{
+				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
+				vel->linear.x = commandsB[0];
+				vel->linear.y = commandsB[1];
+				vel->linear.z = commandsB[2];
+				vel->angular.x = commandsB[3];
+				vel->angular.y = commandsB[4];
+				vel->angular.z = commandsB[5];
+				rosPublisher.publish(vel);
+			}
+			commandsA.clear();
+			commandsB.clear();
+		}
+		commandMutex.unlock();
+
+		ros::spinOnce();
+		loop_rate.sleep();
+	}
+}

rtabmap/src/CameraNode.cpp

@@ -0,0 +1,64 @@
+/*
+ * CameraNode.cpp
+ *
+ *  Created on: 1 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include "CameraWrapper.h"
+#include <rtabmap/core/Camera.h>
+#include <utilite/ULogger.h>
+
+// See the launch file to change the camera values
+#define DEFAULT_DEVICE_ID 0
+#define DEFAULT_IMG_RATE 1 //Hz
+#define DEFAULT_AUTO_RESTART 0
+#define DEFAULT_IMG_WIDTH 0
+#define DEFAULT_IMG_HEIGHT 0
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "camera_node");
+	CameraWrapper * camera = 0;
+	ros::NodeHandle nh;
+
+	int deviceId = DEFAULT_DEVICE_ID;
+	int imgRate = DEFAULT_IMG_RATE;
+	int autoRestart = DEFAULT_AUTO_RESTART;
+	int imgWidth = DEFAULT_IMG_WIDTH;
+	int imgHeight = DEFAULT_IMG_HEIGHT;
+
+	nh.param("cam/device_id", deviceId, deviceId);
+	nh.param("cam/image_rate", imgRate, imgRate);
+	nh.param("cam/auto_restart", autoRestart, autoRestart);
+	nh.param("cam/image_width", imgWidth, imgWidth);
+	nh.param("cam/image_height", imgHeight, imgHeight);
+
+	ROS_INFO("cam/device_id=%d", deviceId);
+	ROS_INFO("cam/image_rate=%d", imgRate);
+	ROS_INFO("cam/auto_restart=%d", autoRestart);
+	ROS_INFO("cam/image_width=%d", imgWidth);
+	ROS_INFO("cam/image_height=%d", imgHeight);
+
+	camera = new CameraVideoWrapper(deviceId, imgRate, autoRestart, imgWidth, imgHeight); // webcam device 0
+
+	if(!camera || (camera && !camera->init()))
+	{
+		ROS_ERROR("Cannot initiate the camera");
+	}
+	else
+	{
+		// Start the camera
+		camera->start();
+		ROS_INFO("Camera started...");
+		ros::spin();
+	}
+
+	//cleanup
+	if(camera)
+	{
+		delete camera;
+	}
+}

rtabmap/src/CameraNodeReceiver.cpp

@@ -0,0 +1,52 @@
+/*
+ * CameraNodeReceiver.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+//#include <sensor_msgs/CompressedImage.h>
+#include <cv_bridge/CvBridge.h>
+#include <highgui.h>
+
+void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
+{
+	// Decompress
+	//const CvMat compressed = cvMat(1, image->data.size(), CV_8UC1, const_cast<unsigned char*>(&image->data[0]));
+	//IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
+
+	//ROS_INFO("Received an image size=(%d,%d)", decompressed->width, decompressed->height);
+	//cvShowImage( "ImageReceived", decompressed );
+	//cvReleaseImage(&decompressed);
+
+	IplImage * image = 0;
+	sensor_msgs::CvBridge bridge;
+	if(msg->data.size())
+	{
+		image = bridge.imgMsgToCv(msg);
+	}
+
+	if(image)
+	{
+		ROS_INFO("Received an image size=(%d,%d)", image->width, image->height);
+		cvShowImage( "ImageReceived", image);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	cvStartWindowThread();
+	cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
+	cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
+
+	ros::init(argc, argv, "camera_node_receiver");
+	ros::NodeHandle n;
+	ros::Subscriber image_sub = n.subscribe("image", 1, imgReceivedCallback);
+
+	ROS_INFO("Waiting for images...");
+
+	ros::spin();
+
+	cvDestroyWindow("ImageReceived");
+}

rtabmap/src/CameraNodeReceiverSM.cpp

@@ -0,0 +1,45 @@
+/*
+ * CameraNodeReceiver.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include "rtabmap/SensoryMotorState.h"
+#include <cv_bridge/CvBridge.h>
+#include <highgui.h>
+
+void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
+{
+	IplImage * image = 0;
+	sensor_msgs::CvBridge bridge;
+	if( msg->image.data.size())
+	{
+		bridge.fromImage(msg->image);
+		image = bridge.toIpl();
+	}
+
+	if(image)
+	{
+		ROS_INFO("Received an image size=(%d,%d)", image->width, image->height);
+		cvShowImage( "ImageReceived", image);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	cvStartWindowThread();
+	cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
+	cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
+
+	ros::init(argc, argv, "camera_node_receiver_sm");
+	ros::NodeHandle n;
+	ros::Subscriber image_sub = n.subscribe("sm_state", 1, smReceivedCallback);
+
+	ROS_INFO("Waiting for Sensorimotor states (containing images)...");
+
+	ros::spin();
+
+	cvDestroyWindow("ImageReceived");
+}

rtabmap/src/CameraWrapper.cpp

@@ -0,0 +1,171 @@
+/*
+ * CameraWrapper.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "CameraWrapper.h"
+#include <cv_bridge/CvBridge.h>
+//#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/Image.h>
+#include <rtabmap/core/CameraEvent.h>
+#include <rtabmap/core/SMState.h>
+#include <utilite/UEventsManager.h>
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/Parameters.h>
+
+//msgs
+#include "rtabmap/SensoryMotorState.h"
+
+CameraWrapper::CameraWrapper() :
+	camera_(0)
+{
+	rosPublisher_ = nh_.advertise<sensor_msgs::Image>("image", 1);
+	changeCameraImgRateSrv_ = nh_.advertiseService("changeCameraImgRate", &CameraWrapper::changeCameraImgRateCallback, this);
+	UEventsManager::addHandler(this);
+}
+
+CameraWrapper::~CameraWrapper()
+{
+}
+
+void CameraWrapper::setCamera(rtabmap::Camera * camera)
+{
+	if(camera)
+	{
+		if(camera_)
+		{
+			delete camera_;
+			camera_ = 0;
+		}
+
+		camera_ = camera;
+	}
+}
+
+// ownership is transferred
+void CameraWrapper::setPostThreatement(rtabmap::CamPostTreatment * strategy)
+{
+	if(camera_ && strategy)
+	{
+		camera_->setPostThreatement(strategy);
+	}
+}
+
+bool CameraWrapper::init()
+{
+	if(camera_)
+	{
+		return camera_->init();
+	}
+	return false;
+}
+
+void CameraWrapper::start()
+{
+	if(camera_)
+	{
+		return camera_->start();
+	}
+}
+
+bool CameraWrapper::changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Request & request, rtabmap::ChangeCameraImgRate::Response & response)
+{
+	nh_.setParam("image_rate", request.imgRate);
+	nh_.setParam("auto_restart", request.autoRestart);
+	UEventsManager::post(new rtabmap::CameraEvent(rtabmap::CameraEvent::kCmdChangeParam, request.imgRate, request.autoRestart));
+	return true;
+}
+
+void CameraWrapper::handleEvent(UEvent* anEvent)
+{
+	if(anEvent->getClassName().compare("SMStateEvent") == 0)
+	{
+		rtabmap::SMStateEvent * e = (rtabmap::SMStateEvent*)anEvent;
+		const rtabmap::SMState * smState = e->getSMState();
+		if(smState && smState->getImage())
+		{
+			try
+			{
+				//int params[3] = {0};
+
+				//JPEG compression
+				//std::string format = "jpeg";
+				//params[0] = CV_IMWRITE_JPEG_QUALITY;
+				//params[1] = 80; // default: 80% quality
+
+				//PNG compression
+				//std::string format = "png";
+				//params[0] = CV_IMWRITE_PNG_COMPRESSION;
+				//params[1] = 9; // default: maximum compression
+
+				//std::string extension = '.' + format;
+
+				// Compress image
+				//const IplImage* image = e->getImage();
+				//CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
+
+				// Set up message and publish
+				//sensor_msgs::CompressedImage compressed;
+				//compressed.format = format;
+				//compressed.data.resize(buf->width);
+				//memcpy(&compressed.data[0], buf->data.ptr, buf->width);
+				//cvReleaseMat(&buf);
+
+				//ROS_INFO("Publishing an image");
+				//rosPublisher_.publish(compressed);
+				sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(smState->getImage());
+				rosPublisher_.publish(msg);
+			}
+			catch (sensor_msgs::CvBridgeException ex)
+			{
+				ROS_ERROR("%s", ex.what());
+			}
+		}
+	}
+}
+
+// Camera Image wrapper
+CameraImagesWrapper::CameraImagesWrapper(
+		const std::string & path,
+		int startAt,
+		bool refreshDir,
+		float imageRate,
+		bool autoRestart,
+		unsigned int imageWidth,
+		unsigned int imageHeight)
+{
+	this->setCamera(new rtabmap::CameraImages(path, startAt, refreshDir, imageRate, autoRestart, imageWidth, imageHeight));
+}
+
+// Camera Video wrapper
+CameraVideoWrapper::CameraVideoWrapper(int usbDevice,
+										float imageRate,
+										bool autoRestart,
+										unsigned int imageWidth,
+										unsigned int imageHeight)
+{
+	this->setCamera(new rtabmap::CameraVideo(usbDevice, imageRate, autoRestart, imageWidth, imageHeight));
+}
+
+
+CameraVideoWrapper::CameraVideoWrapper(const std::string & fileName,
+										float imageRate,
+										bool autoRestart,
+										unsigned int imageWidth,
+										unsigned int imageHeight)
+{
+	this->setCamera(new rtabmap::CameraVideo(fileName, imageRate, autoRestart, imageWidth, imageHeight));
+}
+
+// Camera database wrapper
+CameraDatabaseWrapper::CameraDatabaseWrapper(const std::string & path,
+		bool ignoreChildren,
+		float imageRate,
+		bool autoRestart,
+		unsigned int imageWidth,
+		unsigned int imageHeight)
+{
+	this->setCamera(new rtabmap::CameraDatabase(path, ignoreChildren, imageRate, autoRestart, imageWidth, imageHeight));
+}

rtabmap/src/CameraWrapper.h

@@ -0,0 +1,106 @@
+/*
+ * CameraWrapper.h
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef CAMERAWRAPPER_H_
+#define CAMERAWRAPPER_H_
+
+#include "utilite/UEventsHandler.h"
+#include <ros/ros.h>
+#include "rtabmap/ChangeCameraImgRate.h"
+#include <std_msgs/Empty.h>
+
+namespace Util
+{
+	class Event;
+}
+
+namespace rtabmap
+{
+	class Camera;
+	class CamPostTreatment;
+}
+
+class CameraWrapper : public UEventsHandler
+{
+public:
+	virtual ~CameraWrapper();
+
+	bool init();
+	void start();
+
+	void setPostThreatement(rtabmap::CamPostTreatment * strategy); // ownership is transferred
+	void updateParameters();
+
+protected:
+	CameraWrapper();
+	virtual void handleEvent(UEvent * anEvent);
+	void setCamera(rtabmap::Camera * camera);
+
+private:
+	bool changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Request&, rtabmap::ChangeCameraImgRate::Response&);
+
+private:
+	ros::NodeHandle nh_;
+	ros::Publisher rosPublisher_;
+	rtabmap::Camera * camera_;
+	ros::ServiceServer changeCameraImgRateSrv_;
+};
+
+
+// Specialized wrappers
+
+//Image
+class CameraImagesWrapper : public CameraWrapper
+{
+public:
+	CameraImagesWrapper(const std::string & path,
+						int startAt = 1,
+						bool refreshDir = false,
+						float imageRate = 0,
+						bool autoRestart = false,
+						unsigned int imageWidth = 0,
+						unsigned int imageHeight = 0);
+	virtual ~CameraImagesWrapper() {}
+};
+
+// Video
+class CameraVideoWrapper : public CameraWrapper
+{
+public:
+	// Usb device like a Webcam
+	CameraVideoWrapper(int usbDevice = 0,
+						float imageRate = 0,
+						bool autoRestart = false,
+						unsigned int imageWidth = 0,
+						unsigned int imageHeight = 0);
+
+	// for a video file (AVI)
+	CameraVideoWrapper(const std::string & fileName,
+						float imageRate = 0,
+						bool autoRestart = false,
+						unsigned int imageWidth = 0,
+						unsigned int imageHeight = 0);
+
+	virtual ~CameraVideoWrapper() {}
+};
+
+// Database
+class CameraDatabaseWrapper : public CameraWrapper
+{
+public:
+	// Usb device like a Webcam
+	CameraDatabaseWrapper(const std::string & path,
+			bool ignoreChildren,
+			float imageRate = 0,
+			bool autoRestart = false,
+			unsigned int imageWidth = 0,
+			unsigned int imageHeight = 0);
+
+	virtual ~CameraDatabaseWrapper() {}
+};
+
+#endif /* CAMERAWRAPPER_H_ */

rtabmap/src/CoreNode.cpp

@@ -0,0 +1,35 @@
+/*
+ * CoreNode.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "CoreWrapper.h"
+#include "utilite/ULogger.h"
+
+int main(int argc, char** argv)
+{
+	ROS_INFO("Starting node...");
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ros::init(argc, argv, "core_node");
+	const char* opt_delete_db_on_start    = "--delete_db_on_start";
+
+	bool deleteDbOnStart = false;
+	for(int i=1;i<argc;i++)
+	{
+		if(!strncmp(argv[i], opt_delete_db_on_start, strlen(opt_delete_db_on_start)))
+		{
+			deleteDbOnStart = true;
+		}
+	}
+
+	CoreWrapper rtabmap(deleteDbOnStart);
+	rtabmap.start();
+
+	ROS_INFO("RTAB-Map started...");
+	ros::spin();
+}

rtabmap/src/CoreWrapper.cpp

@@ -0,0 +1,330 @@
+/*
+ * CoreWrapper.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "CoreWrapper.h"
+#include <rtabmap/core/CameraEvent.h>
+#include <ros/ros.h>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/SMState.h>
+#include "utilite/UtiLite.h"
+#include <highgui.h>
+
+//msgs
+#include "rtabmap/RtabmapInfo.h"
+#include "rtabmap/RtabmapInfoEx.h"
+
+using namespace rtabmap;
+
+CoreWrapper::CoreWrapper(bool deleteDbOnStart) : rtabmap_(0)
+{
+	infoPub_ = nh_.advertise<rtabmap::RtabmapInfo>("rtabmap_info", 1);
+	infoExPub_ = nh_.advertise<rtabmap::RtabmapInfoEx>("rtabmap_info_x", 1);
+	parametersLoadedPub_ = nh_.advertise<std_msgs::Empty>("parameters_loaded", 1);
+
+	rtabmap_ = new Rtabmap();
+	loadNodeParameters(rtabmap_->getIniFilePath());
+
+	if(deleteDbOnStart)
+	{
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDeleteMemory));
+	}
+
+	rtabmap_->init();
+
+	imageTopic_ = nh_.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this);
+	parametersUpdatedTopic_ = nh_.subscribe("parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
+
+	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);
+	UEventsManager::addHandler(this);
+}
+
+CoreWrapper::~CoreWrapper()
+{
+	this->saveNodeParameters(rtabmap_->getIniFilePath());
+	delete rtabmap_;
+}
+
+void CoreWrapper::start()
+{
+	rtabmap_->start();
+}
+
+void CoreWrapper::loadNodeParameters(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!");
+	}
+
+	ParametersMap parameters = Parameters::getDefaultParameters();
+	Rtabmap::readParameters(configFile.c_str(), parameters);
+
+	for(ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
+	{
+		nh_.setParam(i->first, i->second);
+	}
+	parametersLoadedPub_.publish(std_msgs::Empty());
+}
+
+void CoreWrapper::saveNodeParameters(const std::string & configFile)
+{
+	ROS_INFO("Saving parameters to %s", configFile.c_str());
+
+	if(!UFile::exists(configFile.c_str()))
+	{
+		ROS_WARN("Config file doesn't exist, a new one will be created.");
+	}
+
+	ParametersMap parameters = Parameters::getDefaultParameters();
+	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);
+
+	std::string databasePath = parameters.at(Parameters::kRtabmapWorkingDirectory())+"/LTM.db";
+	ROS_INFO("Database/long-term memory (%lu MB) is located at %s/LTM.db", UFile::length(databasePath)/1000000, databasePath.c_str());
+}
+
+
+void CoreWrapper::smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
+{
+	IplImage * image = 0;
+	std::list<cv::KeyPoint> keypoints;
+
+	if(msg->image.data.size())
+	{
+		sensor_msgs::CvBridge bridge;
+		bridge.fromImage(msg->image);
+		image = cvCloneImage(bridge.toIpl());
+	}
+
+	for(unsigned int i=0; i<msg->keypoints.size() && i<msg->keypoints.size(); i++)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->keypoints.at(i).angle;
+		pt.response = msg->keypoints.at(i).response;
+		pt.pt.x = msg->keypoints.at(i).ptx;
+		pt.pt.y = msg->keypoints.at(i).pty;
+		pt.size = msg->keypoints.at(i).size;
+		keypoints.push_back(pt);
+	}
+	rtabmap::SMState * smState = new rtabmap::SMState(msg->sensors, msg->sensorStep, msg->actuators, msg->actuatorStep);
+	smState->setImage(image);
+	smState->setKeypoints(keypoints);
+	UEventsManager::post(new SMStateEvent(smState));
+}
+
+bool CoreWrapper::resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
+	return true;
+}
+
+bool CoreWrapper::dumpMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));
+	return true;
+}
+
+bool CoreWrapper::deleteMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDeleteMemory));
+	return true;
+}
+
+bool CoreWrapper::dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
+	return true;
+}
+
+void CoreWrapper::parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
+{
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string value;
+		if(nh_.getParam(iter->first, value))
+		{
+			iter->second = value;
+		}
+	}
+	ROS_INFO("Updating parameters");
+	UEventsManager::post(new ParamEvent(parameters));
+}
+
+void CoreWrapper::handleEvent(UEvent * anEvent)
+{
+	if(anEvent->getClassName().compare("RtabmapEvent") == 0)
+	{
+		RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
+		const Statistics & stat = rtabmapEvent->getStats();
+
+		//prepare ros message
+
+		if(stat.extended())
+		{
+			rtabmap::RtabmapInfoExPtr msg(new rtabmap::RtabmapInfoEx);
+			msg->info.refId = stat.refImageId();
+			if(stat.refImage())
+			{
+				int params[3] = {0};
+
+				//JPEG compression
+				std::string format = "jpeg";
+				params[0] = CV_IMWRITE_JPEG_QUALITY;
+				params[1] = 80; // default: 80% quality
+
+				//PNG compression
+				//std::string format = "png";
+				//params[0] = CV_IMWRITE_PNG_COMPRESSION;
+				//params[1] = 9; // default: maximum compression
+
+				std::string extension = '.' + format;
+
+				// Compress image
+				const IplImage* image = stat.refImage();
+				CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
+
+				// Set up message and publish
+				sensor_msgs::CompressedImage compressed;
+				compressed.format = format;
+				compressed.data.resize(buf->width);
+				memcpy(&compressed.data[0], buf->data.ptr, buf->width);
+				cvReleaseMat(&buf);
+
+				msg->refImage = compressed;
+			}
+			msg->info.loopClosureId = stat.loopClosureId();
+			if(stat.loopClosureImage())
+			{
+				int params[3] = {0};
+
+				//JPEG compression
+				std::string format = "jpeg";
+				params[0] = CV_IMWRITE_JPEG_QUALITY;
+				params[1] = 80; // default: 80% quality
+
+				//PNG compression
+				//std::string format = "png";
+				//params[0] = CV_IMWRITE_PNG_COMPRESSION;
+				//params[1] = 9; // default: maximum compression
+
+				std::string extension = '.' + format;
+
+				// Compress image
+				const IplImage* image = stat.loopClosureImage();
+				CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
+
+				// Set up message and publish
+				sensor_msgs::CompressedImage compressed;
+				compressed.format = format;
+				compressed.data.resize(buf->width);
+				memcpy(&compressed.data[0], buf->data.ptr, buf->width);
+				cvReleaseMat(&buf);
+
+				msg->loopClosureImage = compressed;
+			}
+
+			const std::list<std::vector<float> > & actuators = stat.getActions();
+			if(actuators.size())
+			{
+				msg->info.actuatorStep = actuators.front().size();
+			}
+			for(std::list<std::vector<float> >::const_iterator iter=actuators.begin();iter!=actuators.end();++iter)
+			{
+				if((iter->size() == 0 && msg->info.actuatorStep > 0) || msg->info.actuatorStep % iter->size() != 0)
+				{
+					ROS_ERROR("Actuators must have all the same length.");
+				}
+				msg->info.actuators.insert(msg->info.actuators.end(), iter->begin(), iter->end());
+			}
+
+			//Posterior, likelihood, childCount
+			msg->posteriorKeys = uKeys(stat.posterior());
+			msg->posteriorValues = uValues(stat.posterior());
+			msg->likelihoodKeys = uKeys(stat.likelihood());
+			msg->likelihoodValues = uValues(stat.likelihood());
+			msg->weightsKeys = uKeys(stat.weights());
+			msg->weightsValues = uValues(stat.weights());
+
+			//SURF stuff...
+			msg->refWordsKeys = uListToVector(uKeys(stat.refWords()));
+			msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
+			int index = 0;
+			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
+				i!=stat.refWords().end();
+				++i)
+			{
+				msg->refWordsValues.at(index).angle = i->second.angle;
+				msg->refWordsValues.at(index).response = i->second.response;
+				msg->refWordsValues.at(index).ptx = i->second.pt.x;
+				msg->refWordsValues.at(index).pty = i->second.pt.y;
+				msg->refWordsValues.at(index).size = i->second.size;
+				msg->refWordsValues.at(index).octave = i->second.octave;
+				msg->refWordsValues.at(index).class_id = i->second.class_id;
+				++index;
+			}
+			msg->loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
+			msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
+			index = 0;
+			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
+				i!=stat.loopWords().end();
+				++i)
+			{
+				msg->loopWordsValues.at(index).angle = i->second.angle;
+				msg->loopWordsValues.at(index).response = i->second.response;
+				msg->loopWordsValues.at(index).ptx = i->second.pt.x;
+				msg->loopWordsValues.at(index).pty = i->second.pt.y;
+				msg->loopWordsValues.at(index).size = i->second.size;
+				msg->loopWordsValues.at(index).octave = i->second.octave;
+				msg->loopWordsValues.at(index).class_id = i->second.class_id;
+				++index;
+			}
+
+			// Statistics data
+			msg->statsKeys = uKeys(stat.data());
+			msg->statsValues = uValues(stat.data());
+
+			ROS_INFO("Publishing statistics...");
+			infoExPub_.publish(msg);
+		}
+		else
+		{
+			rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
+			ROS_INFO("Loop closure detected! newId=%d with oldId=%d", stat.refImageId(), stat.loopClosureId());
+			msg->refId = stat.refImageId();
+			msg->loopClosureId = stat.loopClosureId();
+			const std::list<std::vector<float> > & actuators = stat.getActions();
+			if(actuators.size())
+			{
+				msg->actuatorStep = actuators.front().size();
+			}
+			for(std::list<std::vector<float> >::const_iterator iter=actuators.begin();iter!=actuators.end();++iter)
+			{
+				if((iter->size() == 0 && msg->actuatorStep > 0) || msg->actuatorStep % iter->size() != 0)
+				{
+					ROS_ERROR("Actuators must have all the same length.");
+				}
+				msg->actuators.insert(msg->actuators.end(), iter->begin(), iter->end());
+			}
+			infoPub_.publish(msg);
+		}
+	}
+}

rtabmap/src/CoreWrapper.h

@@ -0,0 +1,65 @@
+/*
+ * CoreWrapper.h
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef COREWRAPPER_H_
+#define COREWRAPPER_H_
+
+
+#include <ros/ros.h>
+#include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
+#include <cv_bridge/CvBridge.h>
+#include "utilite/UEventsHandler.h"
+#include <rtabmap/core/RtabmapEvent.h>
+#include <sensor_msgs/Image.h>
+#include "rtabmap/SensoryMotorState.h"
+
+namespace rtabmap
+{
+	class Rtabmap;
+}
+
+class CoreWrapper : public UEventsHandler
+{
+public:
+	CoreWrapper(bool deleteDbOnStart = false);
+	virtual ~CoreWrapper();
+
+	void start();
+
+private:
+	void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg);
+	void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
+
+	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 loadNodeParameters(const std::string & configFile);
+	void saveNodeParameters(const std::string & configFile);
+
+	virtual void handleEvent(UEvent * anEvent);
+
+private:
+	ros::NodeHandle nh_;
+	rtabmap::Rtabmap * rtabmap_;
+	ros::Subscriber imageTopic_;
+	ros::Subscriber compressedImageTopic_;
+	ros::Subscriber parametersUpdatedTopic_;
+	ros::Publisher infoPub_;
+	ros::Publisher infoExPub_;
+	ros::Publisher parametersLoadedPub_;
+	std::string configFile_;
+
+	ros::ServiceServer resetMemorySrv_;
+	ros::ServiceServer dumpMemorySrv_;
+	ros::ServiceServer deleteMemorySrv_;
+	ros::ServiceServer dumpPredictionSrv_;
+};
+
+#endif /* COREWRAPPER_H_ */

rtabmap/src/GuiNode.cpp

@@ -0,0 +1,32 @@
+/*
+ * CoreNode.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include "utilite/ULogger.h"
+
+#include <QApplication>
+#include <rtabmap/gui/MainWindow.h>
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "gui_node");
+
+	GuiWrapper gui(argc, argv);
+
+	// Here start the ROS events loop
+	ros::AsyncSpinner spinner(4); // Use 4 threads
+	spinner.start();
+
+	ROS_INFO("Node 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...");
+	return r;
+}

rtabmap/src/GuiWrapper.cpp

@@ -0,0 +1,231 @@
+/*
+ * GuiWrapper.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include <rtabmap/gui/MainWindow.h>
+#include "PreferencesDialogROS.h"
+#include <QtGui/QApplication>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <cv_bridge/CvBridge.h>
+#include "utilite/UEventsManager.h"
+#include "std_srvs/Empty.h"
+#include "std_msgs/Empty.h"
+#include <rtabmap/core/Parameters.h>
+#include <highgui.h>
+#include <rtabmap/core/CameraEvent.h>
+#include "rtabmap/ChangeCameraImgRate.h"
+
+using namespace rtabmap;
+
+GuiWrapper::GuiWrapper(int & argc, char** argv)
+{
+	infoTopic_ = nh_.subscribe("rtabmap_info", 1, &GuiWrapper::infoReceivedCallback, this);
+	infoExTopic_ = nh_.subscribe("rtabmap_info_x", 1, &GuiWrapper::infoExReceivedCallback, this);
+	app_ = new QApplication(argc, argv);
+	mainWindow_ = new MainWindow(new PreferencesDialogROS());
+	mainWindow_->show();
+	mainWindow_->changeState(MainWindow::kMonitoring);
+	app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );
+
+	resetMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("resetMemory");
+	dumpMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("dumpMemory");
+	dumpPredictionClient_ = nh_.serviceClient<std_srvs::Empty>("dumpPrediction");
+	deleteMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("deleteMemory");
+	changeCameraImgRateClient_ = nh_.serviceClient<rtabmap::ChangeCameraImgRate>("changeCameraImgRate");
+
+	parametersUpdatedPub_ = nh_.advertise<std_msgs::Empty>("parameters_updated", 1);
+
+	UEventsManager::addHandler(this);
+	UEventsManager::addHandler(mainWindow_);
+}
+
+GuiWrapper::~GuiWrapper()
+{
+	delete mainWindow_;
+	delete app_;
+}
+
+int GuiWrapper::exec()
+{
+	return app_->exec();
+}
+
+void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
+{
+	ROS_INFO("Loop closure detected! newId=%d with oldId=%d", msg->refId, msg->loopClosureId);
+	rtabmap::Statistics * stat = new rtabmap::Statistics();
+	stat->setLoopClosureId(msg->loopClosureId);
+	UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
+}
+
+void GuiWrapper::infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
+{
+	ROS_INFO("Statistics received!");
+	sensor_msgs::CvBridge bridge;
+
+	// Map from ROS struct to rtabmap struct
+	rtabmap::Statistics * stat = new rtabmap::Statistics();
+
+	stat->setExtended(true); // Extended
+
+	stat->setRefImageId(msg->info.refId);
+	if(msg->refImage.data.size() > 0)
+	{
+		// Decompress
+		const CvMat compressed = cvMat(1, msg->refImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->refImage.data[0]));
+		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
+		stat->setRefImage(&decompressed);
+	}
+	stat->setLoopClosureId(msg->info.loopClosureId);
+	if(msg->loopClosureImage.data.size() > 0)
+	{
+		// Decompress
+		const CvMat compressed = cvMat(1, msg->loopClosureImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->loopClosureImage.data[0]));
+		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
+		stat->setLoopClosureImage(&decompressed);
+	}
+
+	//Posterior, likelihood, childCount
+	std::map<int, float> mapIntFloat;
+	for(unsigned int i=0; i<msg->posteriorKeys.size() && i<msg->posteriorValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
+	}
+	stat->setPosterior(mapIntFloat);
+	mapIntFloat.clear();
+	for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
+	}
+	stat->setLikelihood(mapIntFloat);
+	std::map<int, int> mapIntInt;
+	for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
+	{
+		mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
+	}
+	stat->setWeights(mapIntInt);
+
+	//SURF stuff...
+	std::multimap<int, cv::KeyPoint> mapIntKeypoint;
+	for(unsigned int i=0; i<msg->refWordsKeys.size() && i<msg->refWordsValues.size(); i++)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->refWordsValues.at(i).angle;
+		pt.response = msg->refWordsValues.at(i).response;
+		//pt.laplacian = msg->refWordsValues.at(i).laplacian;
+		pt.pt.x = msg->refWordsValues.at(i).ptx;
+		pt.pt.y = msg->refWordsValues.at(i).pty;
+		pt.size = msg->refWordsValues.at(i).size;
+		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->refWordsKeys.at(i), pt));
+	}
+	stat->setRefWords(mapIntKeypoint);
+	mapIntKeypoint.clear();
+	for(unsigned int i=0; i<msg->loopWordsKeys.size() && i<msg->loopWordsValues.size(); i++)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->loopWordsValues.at(i).angle;
+		pt.response = msg->loopWordsValues.at(i).response;
+		//pt.laplacian = msg->loopWordsValues.at(i).laplacian;
+		pt.pt.x = msg->loopWordsValues.at(i).ptx;
+		pt.pt.y = msg->loopWordsValues.at(i).pty;
+		pt.size = msg->loopWordsValues.at(i).size;
+		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->loopWordsKeys.at(i), pt));
+	}
+	stat->setLoopWords(mapIntKeypoint);
+
+	// Statistics data
+	for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
+	{
+		stat->addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
+	}
+
+	ROS_INFO("Publishing statistics...");
+	UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
+}
+
+void GuiWrapper::handleEvent(UEvent * anEvent)
+{
+	if(anEvent->getClassName().compare("CameraEvent") == 0)
+	{
+		rtabmap::CameraEvent * camEvent = (rtabmap::CameraEvent *)anEvent;
+		if(camEvent->getCommand() == rtabmap::CameraEvent::kCmdChangeParam)
+		{
+			rtabmap::ChangeCameraImgRate srv;
+			srv.request.imgRate = camEvent->getImageRate();
+			srv.request.autoRestart = camEvent->getAutoRestart();
+			if(!changeCameraImgRateClient_.call(srv))
+			{
+				ROS_WARN("Can't call \"changeCameraImgRate\" service. Ignore this warning if the rtabmap/camera_node is not used...");
+			}
+		}
+		else
+		{
+			ROS_WARN("Only ChangeImgRate command for the camera is supported yet...");
+		}
+	}
+	else if(anEvent->getClassName().compare("ParamEvent") == 0)
+	{
+		const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
+		rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
+		bool modified = false;
+		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
+		{
+			//save only parameters with valid names
+			if(defaultParameters.find((*i).first) != defaultParameters.end())
+			{
+				nh_.setParam((*i).first, (*i).second);
+				modified = true;
+			}
+			else if((*i).first.find('/') != (*i).first.npos)
+			{
+				ROS_WARN("Parameter %s is not used by the rtabmap node.", (*i).first.c_str());
+			}
+		}
+		if(modified)
+		{
+			ROS_INFO("Parameters updated");
+			parametersUpdatedPub_.publish(std_msgs::Empty());
+		}
+	}
+	else if(anEvent->getClassName().compare("RtabmapEventCmd") == 0)
+	{
+		std_srvs::Empty srv;
+		rtabmap::RtabmapEventCmd::Cmd cmd = ((rtabmap::RtabmapEventCmd *)anEvent)->getCmd();
+		if(cmd == rtabmap::RtabmapEventCmd::kCmdDumpMemory)
+		{
+			if(!dumpMemoryClient_.call(srv))
+			{
+				ROS_ERROR("Can't call \"dumpMemory\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdDumpPrediction)
+		{
+			if(!dumpPredictionClient_.call(srv))
+			{
+				ROS_ERROR("Can't call \"dumpPrediction\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
+		{
+			if(!resetMemoryClient_.call(srv))
+			{
+				ROS_ERROR("Can't call \"resetMemory\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdDeleteMemory)
+		{
+			if(!deleteMemoryClient_.call(srv))
+			{
+				ROS_ERROR("Can't call \"deleteMemory\" service");
+			}
+		}
+		else
+		{
+			ROS_WARN("Unknown command...");
+		}
+	}
+}

rtabmap/src/GuiWrapper.h

@@ -0,0 +1,54 @@
+/*
+ * GuiWrapper.h
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef GUIWRAPPER_H_
+#define GUIWRAPPER_H_
+
+#include <ros/ros.h>
+#include "rtabmap/RtabmapInfo.h"
+#include "rtabmap/RtabmapInfoEx.h"
+#include "utilite/UEventsHandler.h"
+
+namespace rtabmap
+{
+	class MainWindow;
+}
+
+class QApplication;
+
+class GuiWrapper : public UEventsHandler
+{
+public:
+	GuiWrapper(int & argc, char** argv);
+	virtual ~GuiWrapper();
+
+	int exec();
+
+protected:
+	virtual void handleEvent(UEvent * anEvent);
+
+private:
+	void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
+	void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & infoExMsg);
+
+private:
+	ros::NodeHandle nh_;
+	ros::Subscriber infoTopic_;
+	ros::Subscriber infoExTopic_;
+	QApplication * app_;
+	rtabmap::MainWindow * mainWindow_;
+
+	ros::ServiceClient resetMemoryClient_;
+	ros::ServiceClient dumpMemoryClient_;
+	ros::ServiceClient changeCameraImgRateClient_;
+	ros::ServiceClient deleteMemoryClient_;
+	ros::ServiceClient dumpPredictionClient_;
+
+	ros::Publisher parametersUpdatedPub_;
+};
+
+#endif /* GUIWRAPPER_H_ */

rtabmap/src/ImageToSensoryMotorStateNode.cpp

@@ -0,0 +1,149 @@
+/*
+ * CameraNode.cpp
+ *
+ *  Created on: 1 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/SMState.h>
+#include <utilite/ULogger.h>
+#include <utilite/UTimer.h>
+#include <cv_bridge/CvBridge.h>
+#include "rtabmap/SensoryMotorState.h"
+#include <std_msgs/Empty.h>
+
+
+rtabmap::CamKeypointTreatment kpThreatment;
+ros::Publisher rosPublisher;
+
+int imgWidth = 0;
+int imgHeight = 0;
+
+void updateParameters()
+{
+	ros::NodeHandle nh;
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string value;
+		if(nh.getParam(iter->first, value))
+		{
+			iter->second = value;
+		}
+	}
+	ROS_INFO("Updating parameters");
+	kpThreatment.parseParameters(parameters);
+}
+
+void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
+{
+	updateParameters();
+}
+
+void parametersLoadedCallback(const std_msgs::EmptyConstPtr & msg)
+{
+	updateParameters();
+}
+
+void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg)
+{
+	IplImage * image = 0;
+	sensor_msgs::CvBridge bridge;
+	if(imgMsg->data.size())
+	{
+		image = bridge.imgMsgToCv(imgMsg);
+		bool resized = false;
+		if(image &&
+		   imgWidth &&
+		   imgHeight &&
+		   imgWidth != image->width &&
+		   imgHeight != image->height)
+		{
+			// declare a destination IplImage object with correct size, depth and channels
+			IplImage * resampledImg = cvCreateImage( cvSize((int)(imgWidth) ,
+												   (int)(imgHeight) ),
+												   image->depth, image->nChannels );
+
+			//use cvResize to resize source to a destination image (linear interpolation)
+			cvResize(image, resampledImg);
+			image = resampledImg;
+			resized = true;
+		}
+
+		if(image)
+		{
+			UTimer timer;
+			rtabmap::SMState * smState = kpThreatment.process(image);
+			ROS_INFO("Time processing image %fs", timer.ticks());
+			if(smState)
+			{
+				rtabmap::SensoryMotorStatePtr msg(new rtabmap::SensoryMotorState);
+
+				std::vector<float> sensors;
+				int sensorStep = 0;
+				smState->getSensorsMerged(sensors, sensorStep);
+				msg->sensors = sensors;
+				msg->sensorStep = sensorStep;
+
+				std::vector<float> actuators;
+				int actuatorStep = 0;
+				smState->getActuatorsMerged(actuators, actuatorStep);
+				msg->actuators = actuators;
+				msg->actuatorStep = actuatorStep;
+
+				if(!resized)
+				{
+					msg->image = *imgMsg;
+				}
+				else
+				{
+					sensor_msgs::CvBridge::fromIpltoRosImage(image, msg->image);
+				}
+				const std::list<cv::KeyPoint> & keypoints = smState->getKeypoints();
+				msg->keypoints = std::vector<rtabmap::KeyPoint>(keypoints.size());
+				int i=0;
+				for(std::list<cv::KeyPoint>::const_iterator iter = keypoints.begin(); iter!=keypoints.end(); ++iter)
+				{
+					msg->keypoints.at(i).angle = iter->angle;
+					msg->keypoints.at(i).octave = iter->octave;
+					msg->keypoints.at(i).ptx = iter->pt.x;
+					msg->keypoints.at(i).pty = iter->pt.y;
+					msg->keypoints.at(i).response = iter->response;
+					msg->keypoints.at(i).size = iter->size;
+					msg->keypoints.at(i).class_id = iter->class_id;
+					++i;
+				}
+
+				rosPublisher.publish(msg);
+			}
+			if(resized)
+			{
+				cvReleaseImage(&image);
+			}
+		}
+	}
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "image_to_sms_node");
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kDebug);
+
+	ros::NodeHandle nh;
+
+	nh.param("resize_image_width", imgWidth, imgWidth);
+	nh.param("resize_image_height", imgHeight, imgHeight);
+
+	ros::Subscriber parametersUpdatedTopic = nh.subscribe("parameters_updated", 1, parametersUpdatedCallback);
+	ros::Subscriber parametersLoadedTopic = nh.subscribe("parameters_loaded", 1, parametersLoadedCallback);
+	ros::Subscriber image_sub = nh.subscribe("image", 1, imgReceivedCallback);
+	rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
+
+	updateParameters();
+
+	ros::spin();
+}

rtabmap/src/InputNode.cpp

@@ -0,0 +1,86 @@
+/*
+ * CameraNode.cpp
+ *
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include "rtabmap/SensoryMotorState.h"
+#include <geometry_msgs/Twist.h>
+#include <utilite/UMutex.h>
+
+UMutex commandMutex;
+std::list<std::vector<float> > commands;
+ros::Publisher rosPublisher;
+
+void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
+{
+	ROS_INFO("Received camera data");
+	std::vector<float> tmp;
+
+	int sizeActions = -1;
+	commandMutex.lock();
+	{
+		for(std::list<std::vector<float> >::iterator iter = commands.begin(); iter!=commands.end();++iter)
+		{
+			tmp.insert(tmp.end(), iter->begin(), iter->end());
+		}
+		sizeActions = commands.size();
+		commands.clear();
+
+	}
+	commandMutex.unlock();
+
+	rtabmap::SensoryMotorStatePtr state(new rtabmap::SensoryMotorState);
+	state->sensors = msg->sensors;
+	state->sensorStep = msg->sensorStep;
+	state->actuators = tmp;
+	state->actuatorStep = 6;
+	state->image = msg->image;
+	state->keypoints = msg->keypoints;
+
+	rosPublisher.publish(state);
+	ROS_INFO("Sensorimotor state sent (sizeActions=%d)", sizeActions);
+}
+
+void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
+{
+	ROS_INFO("Received command velocity linear=(%f,%f,%f) angular=(%f,%f,%f)",
+			msg->linear.x,
+			msg->linear.y,
+			msg->linear.z,
+			msg->angular.x,
+			msg->angular.y,
+			msg->angular.z);
+	commandMutex.lock();
+	{
+		std::vector<float> v(6);
+		v[0] = msg->linear.x;
+		v[1] = msg->linear.y;
+		v[2] = msg->linear.z;
+		v[3] = msg->angular.x;
+		v[4] = msg->angular.y;
+		v[5] = msg->angular.z;
+
+		commands.push_back(v);
+
+		// 10 Hz + 1 max
+		while(commands.size() > 11)
+		{
+			//remove the oldest
+			commands.pop_front();
+		}
+	}
+	commandMutex.unlock();
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "input_node");
+	ros::NodeHandle n;
+	rosPublisher = n.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
+	ros::Subscriber image_sub = n.subscribe("camera_data", 1, smReceivedCallback);
+	ros::Subscriber velocity_sub = n.subscribe("cmd_vel", 1, velocityReceivedCallback);
+
+	ros::spin();
+}

rtabmap/src/OutputNode.cpp

@@ -0,0 +1,79 @@
+/*
+ * CameraNode.cpp
+ *
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include "rtabmap/RtabmapInfo.h"
+#include "rtabmap/RtabmapInfoEx.h"
+#include <geometry_msgs/Twist.h>
+#include <utilite/UMutex.h>
+
+UMutex commandMutex;
+std::vector<float> commands;
+int commandSize = 0;
+int commandIndex = 0;
+ros::Publisher rosPublisher;
+
+void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
+{
+	commandMutex.lock();
+	{
+		commands = msg->actuators;
+		commandSize = msg->actuatorStep;
+		commandIndex = 0;
+		ROS_INFO("Rtabmap's actions received, commandSize=%d", commandSize);
+	}
+	commandMutex.unlock();
+}
+
+void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
+{
+	ROS_INFO("Rtabmap's actions received");
+	commandMutex.lock();
+	{
+		commands = msg->info.actuators;
+		commandSize = msg->info.actuatorStep;
+		ROS_INFO("Rtabmap's actions received, commandSize=%d, id=%d", commandSize, msg->info.refId);
+		commandIndex = 0;
+	}
+	commandMutex.unlock();
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "output_node");
+	ros::NodeHandle n;
+	ros::Subscriber infoTopic;
+	ros::Subscriber infoExTopic;
+	infoTopic = n.subscribe("rtabmap_info", 1, infoReceivedCallback);
+	infoExTopic = n.subscribe("rtabmap_info_x", 1, infoExReceivedCallback);
+	rosPublisher = n.advertise<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
+
+	ros::Rate loop_rate(10); // 10 Hz
+	while(ros::ok())
+	{
+		commandMutex.lock();
+		{
+			if(commandIndex>=0 &&
+				commandSize==6 &&
+				commandIndex + (commandSize-1) < (int)commands.size())
+			{
+				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
+				vel->linear.x = commands[commandIndex++];
+				vel->linear.y = commands[commandIndex++];
+				vel->linear.z = commands[commandIndex++];
+				vel->angular.x = commands[commandIndex++];
+				vel->angular.y = commands[commandIndex++];
+				vel->angular.z = commands[commandIndex++];
+				ROS_INFO("Publishing vel");
+				rosPublisher.publish(vel);
+			}
+		}
+		commandMutex.unlock();
+
+		ros::spinOnce();
+		loop_rate.sleep();
+	}
+}

rtabmap/src/PreferencesDialogROS.cpp

@@ -0,0 +1,104 @@
+/*
+ * PreferencesDialogROS.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "PreferencesDialogROS.h"
+#include <rtabmap/core/Parameters.h>
+#include <QtCore/QDir>
+#include <QtCore/QSettings>
+#include <QtGui/QHBoxLayout>
+#include <QtCore/QTimer>
+#include <QtGui/QLabel>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <QtGui/QMessageBox>
+#include <ros/exceptions.h>
+
+using namespace rtabmap;
+
+PreferencesDialogROS::PreferencesDialogROS()
+{
+}
+
+PreferencesDialogROS::~PreferencesDialogROS()
+{
+
+}
+
+void PreferencesDialogROS::readCameraSettings(const QString & filePath)
+{
+	double imgRate = 0;
+	nh_.getParam("cam/image_rate", imgRate);
+	this->setImgRate(imgRate);
+}
+
+QString PreferencesDialogROS::getParamMessage()
+{
+	return tr("Reading parameters from the ROS server...");
+}
+
+void PreferencesDialogROS::readCoreSettings(const QString & filePath)
+{
+	if(filePath.isEmpty())
+	{
+		ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
+		bool validParameters = true;
+		rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
+		{
+			std::string value;
+			if(nh_.getParam((*i).first,value))
+			{
+				PreferencesDialog::setParameter((*i).first, value);
+			}
+			else
+			{
+				validParameters = false;
+				break;
+			}
+		}
+
+		if(validParameters)
+		{
+			ROS_INFO("Parameters successfully read.");
+		}
+		else
+		{
+			validParameters = false;
+			QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap/core_node may be not started or some parameters won't work...");
+			ROS_ERROR("%s", warning.toStdString().c_str());
+			QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
+		}
+	}
+	else
+	{
+		PreferencesDialog::readCoreSettings(filePath);
+	}
+
+}
+
+void PreferencesDialogROS::writeSettings(const QString & filePath)
+{
+	writeGuiSettings(filePath);
+
+	// This will tell the MainWindow that the
+	//parameters are updated. The MainWindow will send an Event that
+	// will be handled by the GuiWrapper where we will write
+	// parameters in ROS and the rtabmap_node will be notified.
+	if(_parameters.size())
+	{
+		emit settingsChanged(_parameters);
+
+	}
+
+	if(_obsoletePanels)
+	{
+		emit settingsChanged(_obsoletePanels);
+	}
+
+	_parameters = rtabmap::ParametersMap();
+	_obsoletePanels = kPanelDummy;
+}
+

rtabmap/src/PreferencesDialogROS.h

@@ -0,0 +1,33 @@
+/*
+ * PreferencesDialogROS.h
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef PREFERENCESDIALOGROS_H_
+#define PREFERENCESDIALOGROS_H_
+
+#include <ros/ros.h>
+#include <rtabmap/gui/PreferencesDialog.h>
+
+using namespace rtabmap;
+
+class PreferencesDialogROS : public PreferencesDialog
+{
+public:
+	PreferencesDialogROS();
+	virtual ~PreferencesDialogROS();
+
+protected:
+	virtual QString getParamMessage();
+
+	virtual void readCameraSettings(const QString & filePath);
+	virtual void readCoreSettings(const QString & filePath);
+	virtual void writeSettings(const QString & filePath);
+
+private:
+	ros::NodeHandle nh_;
+};
+
+#endif /* PREFERENCESDIALOGROS_H_ */