Merged Audio branch to trunk

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

rtabmap/src/AbtrVelocityNode.cpp

@@ -6,6 +6,7 @@
 
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
+#include <geometry_msgs/TwistStamped.h>
 #include <utilite/UMutex.h>
 #include <queue>
 #include <utilite/ULogger.h>
@@ -103,7 +104,7 @@ int main(int argc, char** argv)
 		velBTopic = nh.subscribe("cmd_vel_b", 1, velocityBReceivedCallback);
 	}
 
-	rosPublisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+	rosPublisher = nh.advertise<geometry_msgs::TwistStamped>("cmd_vel", 1);
 
 	int index = 1;
 	ros::Rate loop_rate(commandsHz); // 10 Hz
@@ -111,24 +112,28 @@ int main(int argc, char** argv)
 	{
 		loop_rate.sleep();
 		ros::spinOnce();
+
+		geometry_msgs::TwistStampedPtr vel(new geometry_msgs::TwistStamped());
+		vel->header.frame_id = "base_link";
+		vel->header.stamp = ros::Time::now();
+
 		// priority for commandsA
 		if(commandsA.size())
 		{
-			geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
-			vel->linear.x = commandsA.front();
+			vel->twist.linear.x = commandsA.front();
 			commandsA.pop();
-			vel->linear.y = commandsA.front();
+			vel->twist.linear.y = commandsA.front();
 			commandsA.pop();
-			vel->linear.z = commandsA.front();
+			vel->twist.linear.z = commandsA.front();
 			commandsA.pop();
-			vel->angular.x = commandsA.front();
+			vel->twist.angular.x = commandsA.front();
 			commandsA.pop();
-			vel->angular.y = commandsA.front();
+			vel->twist.angular.y = commandsA.front();
 			commandsA.pop();
-			vel->angular.z = commandsA.front();
+			vel->twist.angular.z = commandsA.front();
 			commandsA.pop();
 			rosPublisher.publish(vel);
-			UINFO("%d A %f %f %f", index, vel->linear.x, vel->linear.y, vel->angular.z);
+			UINFO("%d A %f %f %f", index, vel->twist.linear.x, vel->twist.linear.y, vel->twist.angular.z);
 			if(!cmdABuffered)
 			{
 				commandsA = std::queue<float>();
@@ -145,20 +150,19 @@ int main(int argc, char** argv)
 		}
 		else if(commandsB.size())
 		{
-			geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
-			vel->linear.x = commandsB.front();
+			vel->twist.linear.x = commandsB.front();
 			commandsB.pop();
-			vel->linear.y = commandsB.front();
+			vel->twist.linear.y = commandsB.front();
 			commandsB.pop();
-			vel->linear.z = commandsB.front();
+			vel->twist.linear.z = commandsB.front();
 			commandsB.pop();
-			vel->angular.x = commandsB.front();
+			vel->twist.angular.x = commandsB.front();
 			commandsB.pop();
-			vel->angular.y = commandsB.front();
+			vel->twist.angular.y = commandsB.front();
 			commandsB.pop();
-			vel->angular.z = commandsB.front();
+			vel->twist.angular.z = commandsB.front();
 			commandsB.pop();
-			UINFO("%d B %f %f %f", index, vel->linear.x, vel->linear.y, vel->angular.z);
+			UINFO("%d B %f %f %f", index, vel->twist.linear.x, vel->twist.linear.y, vel->twist.angular.z);
 			rosPublisher.publish(vel);
 			if(!cmdBBuffered)
 			{
@@ -172,13 +176,12 @@ int main(int argc, char** argv)
 			{
 				// Republish the last command one more time
 				// (if the sender cannot reach commandsHz for an iteration)
-				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
-				vel->linear.x = lastCommandsB[0];
-				vel->linear.y = lastCommandsB[1];
-				vel->linear.z = lastCommandsB[2];
-				vel->angular.x = lastCommandsB[3];
-				vel->angular.y = lastCommandsB[4];
-				vel->angular.z = lastCommandsB[5];
+				vel->twist.linear.x = lastCommandsB[0];
+				vel->twist.linear.y = lastCommandsB[1];
+				vel->twist.linear.z = lastCommandsB[2];
+				vel->twist.angular.x = lastCommandsB[3];
+				vel->twist.angular.y = lastCommandsB[4];
+				vel->twist.angular.z = lastCommandsB[5];
 				rosPublisher.publish(vel);
 				lastCommandsB = std::vector<float>();
 			}

rtabmap/src/CameraNode.cpp

@@ -1,72 +0,0 @@
-/*
- * 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.0 //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");
-	CameraWrapper * camera = 0;
-	ros::NodeHandle nh("~");
-
-	int deviceId = DEFAULT_DEVICE_ID;
-	double imgRate = DEFAULT_IMG_RATE;
-	int autoRestart = DEFAULT_AUTO_RESTART;
-	int imgWidth = DEFAULT_IMG_WIDTH;
-	int imgHeight = DEFAULT_IMG_HEIGHT;
-
-	nh.param("device_id", deviceId, deviceId);
-	nh.param("image_hz", imgRate, imgRate);
-	nh.param("auto_restart", autoRestart, autoRestart);
-	nh.param("image_width", imgWidth, imgWidth);
-	nh.param("image_height", imgHeight, imgHeight);
-
-	ROS_INFO("device_id=%d", deviceId);
-	ROS_INFO("image_hz=%f", imgRate);
-	ROS_INFO("auto_restart=%d", autoRestart);
-	ROS_INFO("image_width=%d", imgWidth);
-	ROS_INFO("image_height=%d", imgHeight);
-
-	nh.setParam("device_id", deviceId);
-	nh.setParam("image_hz", imgRate);
-	nh.setParam("auto_restart", autoRestart);
-	nh.setParam("image_width", imgWidth);
-	nh.setParam("image_height", imgHeight);
-
-	camera = new CameraVideoWrapper(deviceId, float(imgRate), autoRestart, imgWidth, imgHeight); // webcam device 0
-
-	if(!camera || (camera && !camera->init()))
-	{
-		ROS_ERROR("Cannot initiate the camera. Verify if OpenCV is built with ffmpeg support.");
-	}
-	else
-	{
-		// Start the camera
-		camera->start();
-		ROS_INFO("Camera started...");
-		ros::spin();
-	}
-
-	//cleanup
-	if(camera)
-	{
-		delete camera;
-	}
-
-	return 0;
-}

rtabmap/src/CameraNodeReceiver.cpp

@@ -1,85 +0,0 @@
-/*
- * CameraNodeReceiver.cpp
- *
- *  Created on: 2 févr. 2010
- *      Author: labm2414
- */
-
-#include <ros/ros.h>
-#include <cv_bridge/cv_bridge.h>
-#include <image_transport/image_transport.h>
-#include <highgui.h>
-#include <utilite/UDirectory.h>
-#include <utilite/UConversion.h>
-
-bool imagesSaved = false;
-int i = 0;
-
-void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
-{
-	if(msg->data.size())
-	{
-		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
-		IplImage image = ptr->image;
-
-		ROS_INFO("Received an image size=(%d,%d)", image.width, image.height);
-
-		if(imagesSaved)
-		{
-			std::string path = "./imagesSaved";
-			if(!UDirectory::exists(path))
-			{
-				if(!UDirectory::makeDir(path))
-				{
-					ROS_ERROR("Cannot make dir %s", path.c_str());
-				}
-			}
-			path.append("/");
-			path.append(uNumber2Str(i++));
-			path.append(".bmp");
-			if(!cvSaveImage(path.c_str(), &image))
-			{
-				ROS_ERROR("Cannot save image to %s", path.c_str());
-			}
-			else
-			{
-				ROS_INFO("Saved image %s", path.c_str());
-			}
-		}
-		else
-		{
-			cvShowImage( "ImageReceived", &image);
-		}
-	}
-}
-
-int main(int argc, char** argv)
-{
-	ros::init(argc, argv, "camera_receiver");
-	ros::NodeHandle pn("~");
-
-	pn.param("images_saved", imagesSaved, imagesSaved);
-	ROS_INFO("images_saved=%d", imagesSaved?1:0);
-
-	if(!imagesSaved)
-	{
-		cvStartWindowThread();
-		cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
-		cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
-	}
-
-	ros::NodeHandle n;
-	image_transport::ImageTransport it(n);
-	image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
-
-	ROS_INFO("Waiting for images...");
-
-	ros::spin();
-
-	if(!imagesSaved)
-	{
-		cvDestroyWindow("ImageReceived");
-	}
-
-	return 0;
-}

rtabmap/src/CameraNodeReceiverSM.cpp

@@ -1,43 +0,0 @@
-/*
- * CameraNodeReceiver.cpp
- *
- *  Created on: 2 févr. 2010
- *      Author: labm2414
- */
-
-#include <ros/ros.h>
-#include "rtabmap/SensoryMotorState.h"
-#include <cv_bridge/cv_bridge.h>
-#include <highgui.h>
-
-void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
-{
-	if( msg->image.data.size())
-	{
-		boost::shared_ptr<sensor_msgs::Image> tracked_object;
-		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg->image, tracked_object);
-		IplImage image = ptr->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_receiver_sms");
-	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");
-
-	return 0;
-}

rtabmap/src/CameraWrapper.cpp

@@ -1,141 +0,0 @@
-/*
- * CameraWrapper.cpp
- *
- *  Created on: 2 févr. 2010
- *      Author: labm2414
- */
-
-#include "CameraWrapper.h"
-#include <cv_bridge/cv_bridge.h>
-#include <sensor_msgs/image_encodings.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)
-{
-	ros::NodeHandle nh("~");
-	image_transport::ImageTransport it(nh);
-	rosPublisher_ = it.advertise("image", 1);
-	changeCameraImgRateSrv_ = nh.advertiseService("changeImgRate", &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)
-{
-	ros::NodeHandle nh("~");
-	nh.setParam("image_hz", request.imgRate);
-	camera_->setImageRate(request.imgRate);
-	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())
-		{
-			cv_bridge::CvImage img;
-			img.encoding = sensor_msgs::image_encodings::BGR8;
-			img.image = smState->getImage();
-			rosPublisher_.publish(img.toImageMsg());
-		}
-	}
-}
-
-// 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 actionsLoaded,
-		float imageRate,
-		bool autoRestart,
-		unsigned int imageWidth,
-		unsigned int imageHeight)
-{
-	this->setCamera(new rtabmap::CameraDatabase(path, actionsLoaded, imageRate, autoRestart, imageWidth, imageHeight));
-}

rtabmap/src/CameraWrapper.h

@@ -1,106 +0,0 @@
-/*
- * 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>
-#include <image_transport/image_transport.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:
-	image_transport::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 actionsLoaded,
-			float imageRate = 0,
-			bool autoRestart = false,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
-
-	virtual ~CameraDatabaseWrapper() {}
-};
-
-#endif /* CAMERAWRAPPER_H_ */

rtabmap/src/CoreNode.cpp

@@ -6,7 +6,7 @@
  */
 
 #include "CoreWrapper.h"
-#include "utilite/ULogger.h"
+#include <utilite/ULogger.h>
 
 int main(int argc, char** argv)
 {

rtabmap/src/CoreWrapper.cpp

@@ -6,18 +6,23 @@
  */
 
 #include "CoreWrapper.h"
-#include <rtabmap/core/CameraEvent.h>
+#include "MsgConversion.h"
 #include <ros/ros.h>
 #include <rtabmap/core/RtabmapEvent.h>
 #include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Camera.h>
 #include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/SMState.h>
-#include "utilite/UtiLite.h"
-#include <highgui.h>
+#include <rtabmap/core/SensorimotorEvent.h>
+#include <utilite/UEventsManager.h>
+#include <utilite/ULogger.h>
+#include <utilite/UFile.h>
+#include <utilite/UStl.h>
+#include <opencv2/highgui/highgui.hpp>
 
 //msgs
 #include "rtabmap/RtabmapInfo.h"
 #include "rtabmap/RtabmapInfoEx.h"
+#include "rtabmap/CvMatMsg.h"
 
 using namespace rtabmap;
 
@@ -26,9 +31,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 {
 	ros::NodeHandle nh("~");
 	infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1);
+	infoPubEx_ = nh.advertise<rtabmap::RtabmapInfo>("infoEx", 1);
 	parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
 
 	rtabmap_ = new Rtabmap();
+	UEventsManager::addHandler(rtabmap_);
 	loadNodeParameters(rtabmap_->getIniFilePath());
 
 	if(deleteDbOnStart)
@@ -44,8 +51,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	dumpPredictionSrv_ = nh.advertiseService("dumpPrediction", &CoreWrapper::dumpPredictionCallback, this);
 
 	nh = ros::NodeHandle();
-	smStateTopic_ = nh.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this);
 	parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
+	sensorimotorTopic_ = nh.subscribe("sensorimotor", 1, &CoreWrapper::sensorimotorReceivedCallback, this);
 
 	image_transport::ImageTransport it(nh);
 	imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
@@ -109,44 +116,38 @@ void CoreWrapper::saveNodeParameters(const std::string & configFile)
 	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)
+void CoreWrapper::sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg)
 {
-	IplImage * image = 0;
-	std::vector<cv::KeyPoint> keypoints(msg->keypoints.size());
+	std::list<Sensor> sensors;
+	std::list<Actuator> actuators;
 
-	if(msg->image.data.size())
+	for(unsigned int i=0; i<msg->sensors.size(); ++i)
 	{
-		boost::shared_ptr<sensor_msgs::Image> tracked_object;
-		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg->image, tracked_object);
-		IplImage img = ptr->image;
-		image = cvCloneImage(&img);
+		sensors.push_back(Sensor(fromCvMatMsgToCvMat(msg->sensors[i].matrix), (Sensor::Type)msg->sensors[i].type));
+	}
+	for(unsigned int i=0; i<msg->actuators.size(); ++i)
+	{
+		actuators.push_back(Actuator(fromCvMatMsgToCvMat(msg->actuators[i].matrix), (Actuator::Type)msg->actuators[i].type));
 	}
 
-	for(unsigned int i=0; i<msg->keypoints.size() && i<msg->keypoints.size(); i++)
+	if(!sensors.size() && !actuators.size())
 	{
-		keypoints[i].angle = msg->keypoints.at(i).angle;
-		keypoints[i].response = msg->keypoints.at(i).response;
-		keypoints[i].pt.x = msg->keypoints.at(i).ptx;
-		keypoints[i].pt.y = msg->keypoints.at(i).pty;
-		keypoints[i].size = msg->keypoints.at(i).size;
+		ROS_ERROR("Sensorimotor received is empty...");
+	}
+	else
+	{
+		ROS_INFO("Received sensorimotor (%d sensors %d actuators).", sensors.size(), actuators.size());
+		UEventsManager::post(new SensorimotorEvent(sensors, actuators));
 	}
-	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));
 }
 
 void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
 {
 	if(msg->data.size())
 	{
-		boost::shared_ptr<sensor_msgs::Image> tracked_object;
+		ROS_INFO("Received image.");
 		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
-		IplImage imgTmp = ptr->image;
-		IplImage * image = &imgTmp;
-		rtabmap::SMState * smState = new rtabmap::SMState(cvCloneImage(image));
-		UEventsManager::post(new SMStateEvent(smState));
+		UEventsManager::post(new CameraEvent(ptr->image.clone()));
 	}
 }
 
@@ -194,140 +195,106 @@ 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())
+		if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers())
 		{
+			ROS_INFO("Sending RtabmapInfo msg...");
+			RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
+			const Statistics & stat = rtabmapEvent->getStats();
+
 			rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
+
+			// General info
 			msg->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->infoEx.refImage = compressed;
-			}
 			msg->loopClosureId = stat.loopClosureId();
-			if(stat.loopClosureImage())
+
+			msg->actuators.resize(stat.getActuators().size());
+			int i=0;
+			for(std::list<Actuator>::const_iterator iter = stat.getActuators().begin(); iter!=stat.getActuators().end(); ++iter)
 			{
-				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->infoEx.loopClosureImage = compressed;
+				msg->actuators[i].type = iter->type();
+				fromCvMatToCvMatMsg(msg->actuators[i++].matrix, iter->data());
 			}
 
-			const std::list<std::vector<float> > & actuators = stat.getActions();
-			if(actuators.size())
+			if(infoPub_.getNumSubscribers())
 			{
-				msg->actuatorStep = actuators.front().size();
+				infoPub_.publish(msg);
 			}
-			for(std::list<std::vector<float> >::const_iterator iter=actuators.begin();iter!=actuators.end();++iter)
+
+			if(infoPubEx_.getNumSubscribers())
 			{
-				if((iter->size() == 0 && msg->actuatorStep > 0) || msg->actuatorStep % iter->size() != 0)
+				// Detailed info
+				if(stat.extended())
 				{
-					ROS_ERROR("Actuators must have all the same length.");
+					if(stat.refRawData().size())
+					{
+						msg->infoEx.refRawData.resize(stat.refRawData().size());
+						i=0;
+						for(std::list<Sensor>::const_iterator iter = stat.refRawData().begin(); iter!=stat.refRawData().end(); ++iter)
+						{
+							msg->infoEx.refRawData[i].type = iter->type();
+							fromCvMatToCvMatMsg(msg->infoEx.refRawData[i++].matrix, iter->data());
+						}
+					}
+					if(stat.loopClosureRawData().size())
+					{
+						msg->infoEx.loopRawData.resize(stat.loopClosureRawData().size());
+						i=0;
+						for(std::list<Sensor>::const_iterator iter = stat.loopClosureRawData().begin(); iter!=stat.loopClosureRawData().end(); ++iter)
+						{
+							msg->infoEx.loopRawData[i].type = iter->type();
+							fromCvMatToCvMatMsg(msg->infoEx.loopRawData[i++].matrix, iter->data());
+						}
+					}
+
+					//Posterior, likelihood, childCount
+					msg->infoEx.posteriorKeys = uKeys(stat.posterior());
+					msg->infoEx.posteriorValues = uValues(stat.posterior());
+					msg->infoEx.likelihoodKeys = uKeys(stat.likelihood());
+					msg->infoEx.likelihoodValues = uValues(stat.likelihood());
+					msg->infoEx.weightsKeys = uKeys(stat.weights());
+					msg->infoEx.weightsValues = uValues(stat.weights());
+
+					//Features stuff...
+					msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords()));
+					msg->infoEx.refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
+					int index = 0;
+					for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
+						i!=stat.refWords().end();
+						++i)
+					{
+						msg->infoEx.refWordsValues.at(index).angle = i->second.angle;
+						msg->infoEx.refWordsValues.at(index).response = i->second.response;
+						msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x;
+						msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y;
+						msg->infoEx.refWordsValues.at(index).size = i->second.size;
+						msg->infoEx.refWordsValues.at(index).octave = i->second.octave;
+						msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id;
+						++index;
+					}
+
+					msg->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
+					msg->infoEx.loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
+					index = 0;
+					for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
+						i!=stat.loopWords().end();
+						++i)
+					{
+						msg->infoEx.loopWordsValues.at(index).angle = i->second.angle;
+						msg->infoEx.loopWordsValues.at(index).response = i->second.response;
+						msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x;
+						msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y;
+						msg->infoEx.loopWordsValues.at(index).size = i->second.size;
+						msg->infoEx.loopWordsValues.at(index).octave = i->second.octave;
+						msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id;
+						++index;
+					}
+
+					// Statistics data
+					msg->infoEx.statsKeys = uKeys(stat.data());
+					msg->infoEx.statsValues = uValues(stat.data());
 				}
-				msg->actuators.insert(msg->actuators.end(), iter->begin(), iter->end());
+				infoPubEx_.publish(msg);
 			}
-
-			//Posterior, likelihood, childCount
-			msg->infoEx.posteriorKeys = uKeys(stat.posterior());
-			msg->infoEx.posteriorValues = uValues(stat.posterior());
-			msg->infoEx.likelihoodKeys = uKeys(stat.likelihood());
-			msg->infoEx.likelihoodValues = uValues(stat.likelihood());
-			msg->infoEx.weightsKeys = uKeys(stat.weights());
-			msg->infoEx.weightsValues = uValues(stat.weights());
-
-			//SURF stuff...
-			msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords()));
-			msg->infoEx.refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
-			int index = 0;
-			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
-				i!=stat.refWords().end();
-				++i)
-			{
-				msg->infoEx.refWordsValues.at(index).angle = i->second.angle;
-				msg->infoEx.refWordsValues.at(index).response = i->second.response;
-				msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x;
-				msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y;
-				msg->infoEx.refWordsValues.at(index).size = i->second.size;
-				msg->infoEx.refWordsValues.at(index).octave = i->second.octave;
-				msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id;
-				++index;
-			}
-			msg->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
-			msg->infoEx.loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
-			index = 0;
-			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
-				i!=stat.loopWords().end();
-				++i)
-			{
-				msg->infoEx.loopWordsValues.at(index).angle = i->second.angle;
-				msg->infoEx.loopWordsValues.at(index).response = i->second.response;
-				msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x;
-				msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y;
-				msg->infoEx.loopWordsValues.at(index).size = i->second.size;
-				msg->infoEx.loopWordsValues.at(index).octave = i->second.octave;
-				msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id;
-				++index;
-			}
-
-			// SM masks
-			msg->infoEx.refMotionMask = stat.refMotionMask();
-			msg->infoEx.loopMotionMask = stat.loopMotionMask();
-
-			// Statistics data
-			msg->infoEx.statsKeys = uKeys(stat.data());
-			msg->infoEx.statsValues = uValues(stat.data());
-
-			infoPub_.publish(msg);
 		}
 	}
 }

rtabmap/src/CoreWrapper.h

@@ -16,8 +16,9 @@
 #include "utilite/UEventsHandler.h"
 #include <rtabmap/core/RtabmapEvent.h>
 #include <sensor_msgs/Image.h>
-#include "rtabmap/SensoryMotorState.h"
+#include <geometry_msgs/Twist.h>
 #include <image_transport/image_transport.h>
+#include "rtabmap/Sensorimotor.h"
 
 namespace rtabmap
 {
@@ -33,8 +34,9 @@ public:
 	void start();
 
 private:
-	void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg);
+	void sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg);
 	void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg);
+	void twistCallback(const geometry_msgs::TwistConstPtr & msg);
 	void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
 
 	bool resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -49,10 +51,13 @@ private:
 
 private:
 	rtabmap::Rtabmap * rtabmap_;
-	ros::Subscriber smStateTopic_;
+	ros::Subscriber sensorimotorTopic_;
 	image_transport::Subscriber imageTopic_;
+	ros::Subscriber audioFrameFreqSqrdMagnTopic_;
+	ros::Subscriber twistTopic_;
 	ros::Subscriber parametersUpdatedTopic_;
 	ros::Publisher infoPub_;
+	ros::Publisher infoPubEx_;
 	ros::Publisher parametersLoadedPub_;
 	std::string configFile_;
 

rtabmap/src/GuiWrapper.cpp

@@ -6,27 +6,33 @@
  */
 
 #include "GuiWrapper.h"
-#include <rtabmap/gui/MainWindow.h>
-#include "PreferencesDialogROS.h"
+#include "MsgConversion.h"
 #include <QtGui/QApplication>
-#include <rtabmap/core/RtabmapEvent.h>
+
 #include <cv_bridge/cv_bridge.h>
-#include "utilite/UEventsManager.h"
-#include "std_srvs/Empty.h"
-#include "std_msgs/Empty.h"
+#include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
+
+#include <utilite/UEventsManager.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <rtabmap/gui/MainWindow.h>
+#include <rtabmap/core/RtabmapEvent.h>
 #include <rtabmap/core/Parameters.h>
-#include <highgui.h>
-#include <rtabmap/core/CameraEvent.h>
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/Sensor.h>
+#include <rtabmap/core/SensorimotorEvent.h>
+
+#include "PreferencesDialogROS.h"
 #include "rtabmap/ChangeCameraImgRate.h"
-#include "rtabmap/core/SMState.h"
 
 using namespace rtabmap;
 
-GuiWrapper::GuiWrapper(int & argc, char** argv) :
-	nbCommands_(2)
+GuiWrapper::GuiWrapper(int & argc, char** argv)
 {
 	ros::NodeHandle nh;
-	infoTopic_ = nh.subscribe("rtabmap/info", 1, &GuiWrapper::infoReceivedCallback, this);
+	infoTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoReceivedCallback, this);
 	velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
 	app_ = new QApplication(argc, argv);
 	mainWindow_ = new MainWindow(new PreferencesDialogROS());
@@ -42,8 +48,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 
 	nh = ros::NodeHandle("~");
 	parametersUpdatedPub_ = nh.advertise<std_msgs::Empty>("parameters_updated", 1);
-	nh.param("nb_commands", nbCommands_, nbCommands_);
-	ROS_INFO("nb_commands=%d", nbCommands_);
 
 	UEventsManager::addHandler(this);
 	UEventsManager::addHandler(mainWindow_);
@@ -70,21 +74,28 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 	stat->setExtended(true); // Extended
 
 	stat->setRefImageId(msg->refId);
-	if(msg->infoEx.refImage.data.size() > 0)
+	std::list<Sensor> sensors;
+	for(unsigned int i=0; i<msg->infoEx.refRawData.size(); ++i)
 	{
-		// Decompress
-		const CvMat compressed = cvMat(1, msg->infoEx.refImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->infoEx.refImage.data[0]));
-		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
-		stat->setRefImage(&decompressed);
+		Sensor s(fromCvMatMsgToCvMat(msg->infoEx.refRawData[i].matrix), (Sensor::Type)msg->infoEx.refRawData[i].type);
+		if(s.data().total())
+		{
+			sensors.push_back(s);
+		}
 	}
+	stat->setRefRawData(sensors);
+
 	stat->setLoopClosureId(msg->loopClosureId);
-	if(msg->infoEx.loopClosureImage.data.size() > 0)
+	sensors.clear();
+	for(unsigned int i=0; i<msg->infoEx.loopRawData.size(); ++i)
 	{
-		// Decompress
-		const CvMat compressed = cvMat(1, msg->infoEx.loopClosureImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->infoEx.loopClosureImage.data[0]));
-		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
-		stat->setLoopClosureImage(&decompressed);
+		Sensor s(fromCvMatMsgToCvMat(msg->infoEx.loopRawData[i].matrix), (Sensor::Type)msg->infoEx.loopRawData[i].type);
+		if(s.data().total())
+		{
+			sensors.push_back(s);
+		}
 	}
+	stat->setLoopClosureRawData(sensors);
 
 	//Posterior, likelihood, childCount
 	std::map<int, float> mapIntFloat;
@@ -108,12 +119,11 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 
 	//SURF stuff...
 	std::multimap<int, cv::KeyPoint> mapIntKeypoint;
-	for(unsigned int i=0; i<msg->infoEx.refWordsKeys.size() && i<msg->infoEx.refWordsValues.size(); i++)
+	for(unsigned int i=0; i<msg->infoEx.refWordsKeys.size() && i<msg->infoEx.refWordsValues.size(); ++i)
 	{
 		cv::KeyPoint pt;
 		pt.angle = msg->infoEx.refWordsValues.at(i).angle;
 		pt.response = msg->infoEx.refWordsValues.at(i).response;
-		//pt.laplacian = msg->refWordsValues.at(i).laplacian;
 		pt.pt.x = msg->infoEx.refWordsValues.at(i).ptx;
 		pt.pt.y = msg->infoEx.refWordsValues.at(i).pty;
 		pt.size = msg->infoEx.refWordsValues.at(i).size;
@@ -121,12 +131,11 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 	}
 	stat->setRefWords(mapIntKeypoint);
 	mapIntKeypoint.clear();
-	for(unsigned int i=0; i<msg->infoEx.loopWordsKeys.size() && i<msg->infoEx.loopWordsValues.size(); i++)
+	for(unsigned int i=0; i<msg->infoEx.loopWordsKeys.size() && i<msg->infoEx.loopWordsValues.size(); ++i)
 	{
 		cv::KeyPoint pt;
 		pt.angle = msg->infoEx.loopWordsValues.at(i).angle;
 		pt.response = msg->infoEx.loopWordsValues.at(i).response;
-		//pt.laplacian = msg->loopWordsValues.at(i).laplacian;
 		pt.pt.x = msg->infoEx.loopWordsValues.at(i).ptx;
 		pt.pt.y = msg->infoEx.loopWordsValues.at(i).pty;
 		pt.size = msg->infoEx.loopWordsValues.at(i).size;
@@ -134,22 +143,17 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 	}
 	stat->setLoopWords(mapIntKeypoint);
 
-	//SM stuff
-	stat->setRefMotionMask(msg->infoEx.refMotionMask);
-	stat->setLoopMotionMask(msg->infoEx.loopMotionMask);
-
 	//Actions
-	std::list<std::vector<float> > actions;
-	for(unsigned int i=0; i<msg->actuators.size(); i+=msg->actuatorStep)
+	std::list<Actuator> actuators;
+	for(unsigned int i=0; i<msg->actuators.size(); ++i)
 	{
-		std::vector<float> a(msg->actuatorStep);
-		for(unsigned int j=0; j<a.size(); ++j)
+		Actuator a(fromCvMatMsgToCvMat(msg->actuators[i].matrix), (Actuator::Type)msg->actuators[i].type);
+		if(a.data().total())
 		{
-			a[j] = msg->actuators[i+j];
+			actuators.push_back(a);
 		}
-		actions.push_back(a);
 	}
-	stat->setActions(actions);
+	stat->setActuators(actuators);
 
 	// Statistics data
 	for(unsigned int i=0; i<msg->infoEx.statsKeys.size() && i<msg->infoEx.statsValues.size(); i++)
@@ -161,23 +165,19 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 	UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
 }
 
-void GuiWrapper::velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
+void GuiWrapper::velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg)
 {
-	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;
+	cv::Mat data = cv::Mat(1, 6, CV_32F);
+	data.at<float>(0) = (float)msg->twist.linear.x;
+	data.at<float>(1) = (float)msg->twist.linear.y;
+	data.at<float>(2) = (float)msg->twist.linear.z;
+	data.at<float>(3) = (float)msg->twist.angular.x;
+	data.at<float>(4) = (float)msg->twist.angular.y;
+	data.at<float>(5) = (float)msg->twist.angular.z;
 
-	commands_.push_back(v);
-
-	if(commands_.size() == (unsigned int)nbCommands_)
-	{
-		this->post(new SMStateEvent(new SMState(cv::Mat(), commands_)));
-		commands_.clear();
-	}
+	std::list<Actuator> actuators;
+	actuators.push_back(Actuator(data, rtabmap::Actuator::kTypeTwist));
+	this->post(new rtabmap::SensorimotorEvent(std::list<Sensor>(), actuators));
 }
 
 void GuiWrapper::handleEvent(UEvent * anEvent)

rtabmap/src/GuiWrapper.h

@@ -12,7 +12,7 @@
 #include "rtabmap/RtabmapInfo.h"
 #include "rtabmap/RtabmapInfoEx.h"
 #include "utilite/UEventsHandler.h"
-#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/TwistStamped.h>
 
 namespace rtabmap
 {
@@ -34,7 +34,7 @@ protected:
 
 private:
 	void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
-	void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg);
+	void velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg);
 
 private:
 	ros::Subscriber infoTopic_;
@@ -49,9 +49,6 @@ private:
 	ros::ServiceClient dumpPredictionClient_;
 
 	ros::Publisher parametersUpdatedPub_;
-
-	int nbCommands_;
-	std::list<std::vector<float> > commands_;
 };
 
 #endif /* GUIWRAPPER_H_ */

rtabmap/src/ImageAudioInputNode.cpp

@@ -0,0 +1,90 @@
+/*
+ * InputNode.cpp
+ *
+ *  Created on: 2012-05-27
+ *      Author: mathieu
+ */
+
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <sensor_msgs/Image.h>
+#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
+#include "rtabmap/Sensorimotor.h"
+#include <opencv2/core/core.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include "MsgConversion.h"
+
+class ImageAudioInput
+{
+public:
+	ImageAudioInput(ros::NodeHandle n) :
+		n_(n),
+		image_sub(n_, "image", 1),
+		audio_sub(n_, "audioFrameFreqSqrdMagn", 1),
+		sync(MySyncPolicy(10), image_sub, audio_sub)
+	{
+		sync.registerCallback(boost::bind(&ImageAudioInput::imageAudioCallback, this, _1, _2));
+		sensorimotor_pub_ = n_.advertise<rtabmap::Sensorimotor>("sensorimotor",1);
+	}
+
+	void imageAudioCallback(
+			const sensor_msgs::ImageConstPtr& image,
+			const rtabmap_audio::AudioFrameFreqSqrdMagnConstPtr & audio)
+	{
+		if(!image->data.size())
+		{
+			ROS_ERROR("Image is empty...");
+			return;
+		}
+		if(!audio->data.size())
+		{
+			ROS_ERROR("Audio is empty...");
+			return;
+		}
+
+		// Create a sensorimotor msg
+		rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor());
+		sm->header.stamp = ros::Time::now();
+		sm->sensors.resize(2);
+
+		//image
+		cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image);
+		sm->sensors[0].type = rtabmap::Sensor::kTypeImage;
+		fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false);
+
+		//audio
+		cv::Mat dataMat(audio->nChannels, audio->frameLength, CV_32F);
+		memcpy(dataMat.data, audio->data.data(), audio->data.size()*sizeof(float));
+		sm->sensors[1].type = rtabmap::Sensor::kTypeAudioFreqSqrdMagn;
+		fromCvMatToCvMatMsg(sm->sensors[1].matrix, dataMat);
+
+		sensorimotor_pub_.publish(sm);
+	}
+
+private:
+	ros::NodeHandle n_;
+
+	//inputs
+	message_filters::Subscriber<sensor_msgs::Image> image_sub;
+	message_filters::Subscriber<rtabmap_audio::AudioFrameFreqSqrdMagn > audio_sub;
+
+	//synchronization stuff
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, rtabmap_audio::AudioFrameFreqSqrdMagn> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> sync;
+
+	ros::Publisher sensorimotor_pub_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "image_audio_input");
+	ros::NodeHandle n;
+	ImageAudioInput iai(n);
+
+	ros::spin();
+
+	return 0;
+}
+

rtabmap/src/ImageAudioTwistInputNode.cpp

@@ -0,0 +1,107 @@
+/*
+ * InputNode.cpp
+ *
+ *  Created on: 2012-05-27
+ *      Author: mathieu
+ */
+
+
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <sensor_msgs/Image.h>
+#include <geometry_msgs/TwistStamped.h>
+#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
+#include "rtabmap/Sensorimotor.h"
+#include <opencv2/core/core.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include "MsgConversion.h"
+
+class ImageAudioTwistInput
+{
+public:
+	ImageAudioTwistInput(ros::NodeHandle n) :
+		n_(n),
+		image_sub(n_, "image", 1),
+		audio_sub(n_, "audioFrameFreqSqrdMagn", 1),
+		twist_sub(n_, "cmd_vel", 1),
+		sync(MySyncPolicy(10), image_sub, audio_sub, twist_sub)
+	{
+		sync.registerCallback(boost::bind(&ImageAudioTwistInput::callback, this, _1, _2, _3));
+		sensorimotor_pub_ = n_.advertise<rtabmap::Sensorimotor>("sensorimotor",1);
+	}
+
+	void callback(
+			const sensor_msgs::ImageConstPtr& image,
+			const rtabmap_audio::AudioFrameFreqSqrdMagnConstPtr & audio,
+			const geometry_msgs::TwistStampedConstPtr & twist)
+	{
+		if(!image->data.size())
+		{
+			ROS_ERROR("Image is empty...");
+			return;
+		}
+		if(!audio->data.size())
+		{
+			ROS_ERROR("Audio is empty...");
+			return;
+		}
+
+		// Create a sensorimotor msg
+		cv::Mat data;
+		rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor());
+		sm->header.stamp = ros::Time::now();
+		sm->sensors.resize(2);
+
+		//image
+		cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image);
+		sm->sensors[0].type = rtabmap::Sensor::kTypeImage;
+		fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false);
+
+		//audio
+		data = cv::Mat(audio->nChannels, audio->frameLength, CV_32F);
+		memcpy(data.data, audio->data.data(), audio->data.size()*sizeof(float));
+		sm->sensors[1].type = rtabmap::Sensor::kTypeAudioFreqSqrdMagn;
+		fromCvMatToCvMatMsg(sm->sensors[1].matrix, data);
+
+		//twist
+		sm->actuators.resize(1);
+		data = cv::Mat(1, 6, CV_32F);
+		data.at<float>(0) = (float)twist->twist.linear.x;
+		data.at<float>(1) = (float)twist->twist.linear.y;
+		data.at<float>(2) = (float)twist->twist.linear.z;
+		data.at<float>(3) = (float)twist->twist.angular.x;
+		data.at<float>(4) = (float)twist->twist.angular.y;
+		data.at<float>(5) = (float)twist->twist.angular.z;
+		sm->actuators[0].type = rtabmap::Actuator::kTypeTwist;
+		fromCvMatToCvMatMsg(sm->actuators[0].matrix, data);
+
+		sensorimotor_pub_.publish(sm);
+	}
+
+private:
+	ros::NodeHandle n_;
+
+	//inputs
+	message_filters::Subscriber<sensor_msgs::Image> image_sub;
+	message_filters::Subscriber<rtabmap_audio::AudioFrameFreqSqrdMagn> audio_sub;
+	message_filters::Subscriber<geometry_msgs::TwistStamped> twist_sub;
+
+	//synchronization stuff
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, rtabmap_audio::AudioFrameFreqSqrdMagn, geometry_msgs::TwistStamped> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> sync;
+
+	ros::Publisher sensorimotor_pub_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "image_audio_twist_input");
+	ros::NodeHandle n;
+	ImageAudioTwistInput iati(n);
+
+	ros::spin();
+
+	return 0;
+}

rtabmap/src/ImageToSensorimotorStateNode.cpp

@@ -1,188 +0,0 @@
-/*
- * 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/cv_bridge.h>
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/image_encodings.h>
-#include "rtabmap/SensoryMotorState.h"
-#include <std_msgs/Empty.h>
-#include <opencv2/imgproc/imgproc_c.h>
-
-rtabmap::CamKeypointTreatment kpThreatment;
-ros::Publisher rosPublisher;
-
-int imgWidth = 0;
-int imgHeight = 0;
-double imgRate = 0.0;
-int keypointsExtracted = 0;
-
-UTimer timer;
-
-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)
-{
-	double period = 0.0;
-	if(imgRate > 0)
-	{
-		period = 1.0/imgRate;
-		period -= 0.01 * period; // 1% error
-	}
-	double elapsed = timer.getElapsedTime();
-	if(imgRate == 0.0 || elapsed > period)
-	{
-		timer.start();
-
-		if(imgMsg->data.size())
-		{
-			boost::shared_ptr<sensor_msgs::Image> tracked_object;
-			cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(imgMsg);
-			IplImage imgTmp = ptr->image;
-			IplImage * image = &imgTmp;
-
-			bool resized = false;
-			if(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)
-			{
-				rtabmap::SMState * smState = new rtabmap::SMState(image);
-				if(keypointsExtracted)
-				{
-					kpThreatment.process(smState);
-				}
-				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
-					{
-						cv_bridge::CvImage img;
-						img.encoding = sensor_msgs::image_encodings::BGR8;
-						img.image = image;
-						msg->image = *img.toImageMsg();
-					}
-					const std::vector<cv::KeyPoint> & keypoints = smState->getKeypoints();
-					msg->keypoints = std::vector<rtabmap::KeyPoint>(keypoints.size());
-					int i=0;
-					for(std::vector<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);
-				}
-			}
-		}
-	}
-	else
-	{
-		//ROS_INFO("Ignored frame...");
-	}
-}
-
-int main(int argc, char** argv)
-{
-	ros::init(argc, argv, "image_to_sms");
-	//ULogger::setType(ULogger::kTypeConsole);
-	//ULogger::setLevel(ULogger::kDebug);
-
-	ros::NodeHandle np("~");
-
-	np.param("image_hz", imgRate, imgRate);
-	np.param("resize_image_width", imgWidth, imgWidth);
-	np.param("resize_image_height", imgHeight, imgHeight);
-	np.param("keypoints_extracted", keypointsExtracted, keypointsExtracted);
-
-	ROS_INFO("image_hz=%f\nresize_image_width=%d\nresize_image_height=%d\nkeypointsExtracted=%d", imgRate, imgWidth, imgHeight, keypointsExtracted);
-
-	ros::NodeHandle nh;
-	ros::Subscriber parametersUpdatedTopic = nh.subscribe("rtabmap_gui/parameters_updated", 1, parametersUpdatedCallback);
-	ros::Subscriber parametersLoadedTopic = nh.subscribe("rtabmap/parameters_loaded", 1, parametersLoadedCallback);
-	rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
-
-	image_transport::ImageTransport it(nh);
-	image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
-
-	updateParameters();
-
-	timer.start();
-
-	ros::spin();
-
-	return 0;
-}

rtabmap/src/ImageTwistInputNode.cpp

@@ -0,0 +1,94 @@
+/*
+ * InputNode.cpp
+ *
+ *  Created on: 2012-05-27
+ *      Author: mathieu
+ */
+
+
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <sensor_msgs/Image.h>
+#include <geometry_msgs/TwistStamped.h>
+#include "rtabmap/Sensorimotor.h"
+#include <opencv2/core/core.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include "MsgConversion.h"
+
+class ImageAudioTwistInput
+{
+public:
+	ImageAudioTwistInput(ros::NodeHandle n) :
+		n_(n),
+		image_sub(n_, "image", 1),
+		twist_sub(n_, "cmd_vel", 1),
+		sync(MySyncPolicy(10), image_sub, twist_sub)
+	{
+		sync.registerCallback(boost::bind(&ImageAudioTwistInput::callback, this, _1, _2));
+		sensorimotor_pub_ = n_.advertise<rtabmap::Sensorimotor>("sensorimotor",1);
+	}
+
+	void callback(
+			const sensor_msgs::ImageConstPtr& image,
+			const geometry_msgs::TwistStampedConstPtr & twist)
+	{
+		if(!image->data.size())
+		{
+			ROS_ERROR("Image is empty...");
+			return;
+		}
+
+		// Create a sensorimotor msg
+		cv::Mat data;
+		rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor());
+		sm->header.stamp = ros::Time::now();
+		sm->sensors.resize(2);
+
+		//image
+		cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image);
+		sm->sensors[0].type = rtabmap::Sensor::kTypeImage;
+		fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false);
+
+		//twist
+		sm->actuators.resize(1);
+		data = cv::Mat(1, 6, CV_32F);
+		data.at<float>(0) = (float)twist->twist.linear.x;
+		data.at<float>(1) = (float)twist->twist.linear.y;
+		data.at<float>(2) = (float)twist->twist.linear.z;
+		data.at<float>(3) = (float)twist->twist.angular.x;
+		data.at<float>(4) = (float)twist->twist.angular.y;
+		data.at<float>(5) = (float)twist->twist.angular.z;
+		sm->actuators[0].type = rtabmap::Actuator::kTypeTwist;
+		fromCvMatToCvMatMsg(sm->actuators[0].matrix, data);
+		sm->sensors[1].type = rtabmap::Sensor::kTypeTwist;
+		sm->sensors[1].matrix = sm->actuators[0].matrix;
+
+		sensorimotor_pub_.publish(sm);
+	}
+
+private:
+	ros::NodeHandle n_;
+
+	//inputs
+	message_filters::Subscriber<sensor_msgs::Image> image_sub;
+	message_filters::Subscriber<geometry_msgs::TwistStamped> twist_sub;
+
+	//synchronization stuff
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::TwistStamped> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> sync;
+
+	ros::Publisher sensorimotor_pub_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "image_twist_input");
+	ros::NodeHandle n;
+	ImageAudioTwistInput iati(n);
+
+	ros::spin();
+
+	return 0;
+}

rtabmap/src/InputNode.cpp

@@ -1,138 +0,0 @@
-/*
- * CameraNode.cpp
- *
- *      Author: labm2414
- */
-
-#include <ros/ros.h>
-#include "rtabmap/SensoryMotorState.h"
-#include <geometry_msgs/Twist.h>
-#include <utilite/UMutex.h>
-#include <utilite/UTimer.h>
-#include <utilite/ULogger.h>
-
-UMutex commandMutex;
-std::list<std::vector<float> > commands;
-ros::Publisher rosPublisher;
-UTimer timer;
-rtabmap::SensoryMotorStatePtr state;
-
-bool stateUpdated = false;
-bool actionsUpdated = false;
-int nbCommands = 10;
-bool statsLogged = true;
-const char * statsFileName = "InputStats.txt";
-int index2 = 1;
-
-void publish()
-{
-	if(stateUpdated && actionsUpdated)
-	{
-		std::vector<float> actions;
-		int sizeActions = -1;
-		for(std::list<std::vector<float> >::iterator iter = commands.begin(); iter!=commands.end();++iter)
-		{
-			UINFO("%d %f %f %f", index2, iter->at(0), iter->at(1), iter->at(5));
-			actions.insert(actions.end(), iter->begin(), iter->end());
-		}
-		sizeActions = commands.size();
-		commands.clear();
-
-		state->actuators = actions;
-
-		rosPublisher.publish(state);
-		float elapsed = timer.ticks();
-		ROS_INFO("Sensorimotor state sent (sizeActions=%d, %f Hz)", sizeActions, elapsed>0?1/elapsed:0);
-		stateUpdated = false;
-		actionsUpdated = false;
-		++index2;
-	}
-}
-
-void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
-{
-	state = rtabmap::SensoryMotorStatePtr(new rtabmap::SensoryMotorState);
-	state->sensors = msg->sensors;
-	state->sensorStep = msg->sensorStep;
-	state->actuatorStep = 6;
-	state->image = msg->image;
-	state->keypoints = msg->keypoints;
-
-	stateUpdated = true;
-	publish();
-}
-
-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);
-
-	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 max
-	while(commands.size() > (unsigned int)nbCommands)
-	{
-		UINFO("Ignored %f %f %f", commands.front().at(0), commands.front().at(1), commands.front().at(5));
-		ROS_WARN("Too many commands (%zu) > %d, removing the oldest...", commands.size(), nbCommands);
-		//remove the oldest
-		commands.pop_front();
-	}
-
-	if(commands.size() == (unsigned int)nbCommands)
-	{
-		actionsUpdated = true;
-		publish();
-	}
-}
-
-int main(int argc, char** argv)
-{
-	ros::init(argc, argv, "rtabmap_in");
-	ros::NodeHandle nh("~");
-
-	nh.param("nb_commands", nbCommands, nbCommands);
-	ROS_INFO("nb_commands=%d", nbCommands);
-
-	if(statsLogged)
-	{
-		ULogger::setPrintWhere(false);
-		ULogger::setBuffered(true);
-		ULogger::setPrintLevel(false);
-		ULogger::setPrintTime(false);
-		ULogger::setType(ULogger::kTypeFile, statsFileName, false);
-		ROS_INFO("stats log file = \"%s\"", statsFileName);
-	}
-	else
-	{
-		ULogger::setLevel(ULogger::kError);
-	}
-
-	nh = ros::NodeHandle();
-	rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
-	ros::Subscriber image_sub = nh.subscribe("sensor_data", 1, smReceivedCallback);
-	ros::Subscriber velocity_sub = nh.subscribe("cmd_vel", 1, velocityReceivedCallback);
-
-	timer.start();
-
-	ros::spin();
-
-	if(statsLogged)
-	{
-		ULogger::flush();
-	}
-
-	return 0;
-}

rtabmap/src/MsgConversion.h

@@ -0,0 +1,68 @@
+/*
+ * MsgConversion.h
+ *
+ *  Created on: 2012-05-27
+ *      Author: mathieu
+ */
+
+#ifndef MSGCONVERSION_H_
+#define MSGCONVERSION_H_
+
+#include <rtabmap/core/Sensor.h>
+#include <rtabmap/core/Actuator.h>
+#include "rtabmap/CvMatMsg.h"
+#include "rtabmap/SensorMsg.h"
+#include "rtabmap/ActuatorMsg.h"
+#include <opencv2/highgui/highgui.hpp>
+
+using namespace rtabmap;
+
+cv::Mat fromCvMatMsgToCvMat(const CvMatMsg & matrixMsg)
+{
+	cv::Mat data;
+	if(matrixMsg.compressed)
+	{
+		data = cv::imdecode(matrixMsg.data, -1);
+		if(data.cols != (int)matrixMsg.width || data.rows != (int)matrixMsg.height)
+		{
+			ROS_ERROR("Uncompressed size (%d/%d) is not %d/%d", data.cols, data.rows, matrixMsg.width, matrixMsg.height);
+			data = cv::Mat();
+		}
+	}
+	else
+	{
+		data = cv::Mat(matrixMsg.height, matrixMsg.width, matrixMsg.dataType);
+		if(data.total() * data.elemSize() != matrixMsg.data.size())
+		{
+			ROS_ERROR("Size attributes (total size=%d) is not equal to actual data size (%d)", data.total() * data.elemSize(), matrixMsg.data.size());
+			data = cv::Mat();
+		}
+		else
+		{
+			memcpy(data.data, matrixMsg.data.data(), matrixMsg.data.size());
+		}
+	}
+	return data;
+}
+
+void fromCvMatToCvMatMsg(CvMatMsg & matrixMsg, const cv::Mat & matrix, bool compressImage = false)
+{
+	matrixMsg.width = matrix.cols;
+	matrixMsg.height = matrix.rows;
+	matrixMsg.dataType = matrix.type();
+	if(compressImage)
+	{
+		// compress images
+		matrixMsg.compressed = true;
+		cv::imencode(".png", matrix, matrixMsg.data);
+	}
+	else
+	{
+		matrixMsg.data.resize(matrix.total()*matrix.elemSize());
+		memcpy(matrixMsg.data.data(), matrix.data, matrixMsg.data.size());
+		matrixMsg.compressed = false;
+	}
+}
+
+
+#endif /* MSGCONVERSION_H_ */

rtabmap/src/OutputNode.cpp

@@ -5,6 +5,7 @@
  */
 
 #include <ros/ros.h>
+#include <rtabmap/core/Actuator.h>
 #include "rtabmap/RtabmapInfo.h"
 #include "rtabmap/RtabmapInfoEx.h"
 #include <geometry_msgs/Twist.h>
@@ -14,32 +15,32 @@
 ros::Publisher rosPublisher;
 bool statsLogged = true;
 const char * statsFileName = "OuputStats.txt";
-int index2 = 1;
-
-void publishCommands(const std::vector<float> & commands, int commandSize)
-{
-	if(commandSize && commandSize%6 == 0)
-	{
-		unsigned int commandIndex = 0;
-		while(commandIndex < 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++];
-			UINFO("%d %f %f %f", index2, vel->linear.x, vel->linear.y, vel->angular.z);
-			rosPublisher.publish(vel);
-		}
-		++index2;
-	}
-}
 
 void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
 {
-	publishCommands(msg->actuators, msg->actuatorStep);
+	for(unsigned int i=0; i<msg->actuators.size(); ++i)
+	{
+		if(msg->actuators[i].type == rtabmap::Actuator::kTypeTwist)
+		{
+			if((msg->actuators[i].matrix.dataType & CV_32F) && msg->actuators[i].matrix.data.size()/sizeof(float) == 6)
+			{
+				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
+				float * twist = (float *)msg->actuators[i].matrix.data.data();
+				vel->linear.x = twist[0];
+				vel->linear.y = twist[1];
+				vel->linear.z = twist[2];
+				vel->angular.x = twist[3];
+				vel->angular.y = twist[4];
+				vel->angular.z = twist[5];
+				UINFO("%f %f %f", vel->linear.x, vel->linear.y, vel->angular.z);
+				rosPublisher.publish(vel);
+			}
+			else
+			{
+				ROS_ERROR("Twist format is wrong...");
+			}
+		}
+	}
 }
 
 int main(int argc, char** argv)

rtabmap/src/PreferencesDialogROS.cpp

@@ -32,7 +32,7 @@ void PreferencesDialogROS::readCameraSettings(const QString & filePath)
 	double imgRate = 0;
 	ros::NodeHandle nh;
 	nh.getParam("camera/image_hz", imgRate);
-	this->setImgRate(imgRate);
+	this->setInputRate(imgRate);
 }
 
 QString PreferencesDialogROS::getParamMessage()

rtabmap/src/Twist2Pose.cpp

@@ -44,12 +44,12 @@ void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
 	}
 	msgLinear.header.frame_id = "/base_link";
 	msgAngular.header.frame_id = "/base_link";
+	msgLinear.header.stamp = ros::Time::now();
+	msgAngular.header.stamp = ros::Time::now();
 	rosPublisherLinear.publish(msgLinear);
 	rosPublisherAngular.publish(msgAngular);
 }
 
-#include <ros/ros.h>
-
 int main(int argc, char * argv[])
 {
 	ros::init(argc, argv, "twist_to_poses");
@@ -57,7 +57,7 @@ int main(int argc, char * argv[])
 	ros::NodeHandle nh;
 	rosPublisherLinear = nh.advertise<geometry_msgs::PoseStamped>("pose_linear", 1);
 	rosPublisherAngular = nh.advertise<geometry_msgs::PoseStamped>("pose_angular", 1);
-	ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
+	ros::Subscriber twist_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
 
 	ros::spin();
 

rtabmap/src/Twist2TwistStamped.cpp

@@ -0,0 +1,35 @@
+/*
+ * TwistToPoses.cpp
+ *
+ *  Created on: 2011-11-30
+ *      Author: matlab
+ */
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/TwistStamped.h>
+
+ros::Publisher pub;
+
+void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
+{
+	geometry_msgs::TwistStamped twistStamped;
+
+	twistStamped.twist = *msg;
+	twistStamped.header.frame_id = "/base_link";
+	twistStamped.header.stamp = ros::Time::now();
+	pub.publish(twistStamped);
+}
+
+int main(int argc, char * argv[])
+{
+	ros::init(argc, argv, "twist_to_twist_stamped");
+
+	ros::NodeHandle nh;
+	pub = nh.advertise<geometry_msgs::TwistStamped>("cmd_vel_stamped", 1);
+	ros::Subscriber twist_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
+
+	ros::spin();
+
+	return 0;
+}

rtabmap/src/TwistToSensorimotorStateNode.cpp

@@ -1,41 +0,0 @@
-/*
- * CameraNode.cpp
- *
- *  Created on: 1 févr. 2010
- *      Author: labm2414
- */
-
-#include <ros/ros.h>
-#include "rtabmap/SensoryMotorState.h"
-#include <geometry_msgs/Twist.h>
-
-ros::Publisher rosPublisher;
-
-void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
-{
-	std::vector<float> v(6);
-	v[0] = msg->linear.x*100;
-	v[1] = msg->linear.y*100;
-	v[2] = msg->linear.z*100;
-	v[3] = msg->angular.x*100;
-	v[4] = msg->angular.y*100;
-	v[5] = msg->angular.z*100;
-
-	rtabmap::SensoryMotorStatePtr smMsg(new rtabmap::SensoryMotorState);
-	smMsg->sensors = v;
-	smMsg->sensorStep = v.size();
-	rosPublisher.publish(smMsg);
-}
-
-int main(int argc, char** argv)
-{
-	ros::init(argc, argv, "twist_to_sms");
-
-	ros::NodeHandle nh;
-	rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
-	ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
-
-	ros::spin();
-
-	return 0;
-}

rtabmap/src/VisualAttentionNode.cpp

@@ -0,0 +1,507 @@
+/*
+ * VisualAttentionNode.cpp
+ */
+
+#include <ros/ros.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <signal.h>
+#include <utilite/UPlot.h>
+#include <QtGui/QApplication>
+#include <QtGui/QSpinBox>
+#include <QtGui/QCheckBox>
+#include <QtGui/QVBoxLayout>
+#include <opencv2/imgproc/imgproc_c.h>
+
+image_transport::Publisher rosPublisherMotionGlobal;
+image_transport::Publisher rosPublisherMotionLocal;
+image_transport::Publisher rosPublisherLocal;
+image_transport::Publisher rosPublisherMotionLocalPolar;
+image_transport::Publisher rosPublisherMotionLocalPolarReconstructed;
+image_transport::Publisher rosPublisherLocalPolar;
+image_transport::Publisher rosPublisherLocalPolarReconstructed;
+image_transport::Publisher rosPublisherWithRoi;
+UPlotCurve * g_curveR = 0;
+UPlotCurve * g_curveG = 0;
+UPlotCurve * g_curveB = 0;
+
+UPlotCurve * g_curveRRatio = 0;
+UPlotCurve * g_curveGRatio = 0;
+UPlotCurve * g_curveBRatio = 0;
+
+QSpinBox * spinX = 0;
+QSpinBox * spinY = 0;
+QCheckBox * polarCheckBox = 0;
+
+#define ROI_RATIO 4 // default 8
+#define POLAR_RAYS 128
+#define POLAR_RINGS 64
+cv::Mat previousGlobalImage;
+cv::Mat previousPolarROI;
+cv::Rect roi;
+float ratio = 0.2;
+bool attentionDisabled = false; // may set ROI_RATIO=1 if true
+bool localAttention = false;
+
+void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
+{
+	if(msg->data.size())
+	{
+		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
+
+		if(ptr->image.depth() == CV_8U && ptr->image.channels() == 3)
+		{
+			cv::Mat motion = ptr->image.clone();
+			if(previousGlobalImage.cols == ptr->image.cols && previousGlobalImage.rows == ptr->image.rows)
+			{
+				unsigned char * imageData = (unsigned char *)motion.data;
+				unsigned char * previous_imageData = (unsigned char *)previousGlobalImage.data;
+				int widthStep = motion.cols * motion.elemSize();
+				cv::Point2i centerROILocal(roi.width/2, roi.height/2);
+				cv::Point2i centerROIGlobal(roi.x+roi.width/2, roi.y+roi.height/2);
+				cv::Point2i nearestMovingPixel;
+
+				for(int j=0; j<motion.rows; ++j)
+				{
+					for(int i=0; i<motion.cols; ++i)
+					{
+						float b = (float)imageData[j*widthStep+i*3+0];
+						float g = (float)imageData[j*widthStep+i*3+1];
+						float r = (float)imageData[j*widthStep+i*3+2];
+						float previous_b = (float)previous_imageData[j*widthStep+i*3+0];
+						float previous_g = (float)previous_imageData[j*widthStep+i*3+1];
+						float previous_r = (float)previous_imageData[j*widthStep+i*3+2];
+
+						if(!(fabs(b-previous_b)/256.0f>=ratio || fabs(g-previous_g)/256.0f >= ratio || fabs(r-previous_r)/256.0f >= ratio))
+						{
+							imageData[j*widthStep+i*3+0] = 0;
+							imageData[j*widthStep+i*3+1] = 0;
+							imageData[j*widthStep+i*3+2] = 0;
+						}
+					}
+				}
+
+				// Motion ROI
+				cv::Mat motionROI = cv::Mat(motion, roi);
+				if(rosPublisherMotionLocal.getNumSubscribers())
+				{
+					cv_bridge::CvImage img;
+					img.header.frame_id = msg->header.frame_id;
+					img.header.stamp = msg->header.stamp;
+					img.encoding = sensor_msgs::image_encodings::BGR8;
+					img.image = motionROI;
+					rosPublisherMotionLocal.publish(img.toImageMsg());
+				}
+
+				/*{
+					int radius = motionROI.cols/2;
+					float M = 64/std::log(radius);
+					cv::Mat tmpPolar(128, 64, CV_8UC3);
+					IplImage iplTmpPolar = tmpPolar;
+					IplImage iplPolar = motionROI;
+					cvLogPolar( &iplPolar, &iplTmpPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
+					cvLogPolar( &iplTmpPolar, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
+
+					if(rosPublisherMotionLocalPolar.getNumSubscribers())
+					{
+						cv_bridge::CvImage img;
+						img.encoding = sensor_msgs::image_encodings::BGR8;
+						img.image = motionROI;
+						rosPublisherMotionLocalPolar.publish(img.toImageMsg());
+					}
+				}*/
+
+
+				// Motion ROI polar
+				{
+					cv::Mat imageROI = cv::Mat(ptr->image, roi);
+					int radius = imageROI.cols/2;
+					float M = POLAR_RINGS/std::log(radius);
+					cv::Mat polarROI(POLAR_RAYS, POLAR_RINGS, CV_8UC3);
+					IplImage iplPolar = polarROI;
+					IplImage iplImageROI = imageROI;
+					cvLogPolar( &iplImageROI, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
+					cv::Mat motionPolarROI = polarROI.clone();
+					if(previousPolarROI.cols == motionPolarROI.cols && previousPolarROI.rows == motionPolarROI.rows)
+					{
+						unsigned char * motionPolarData = motionPolarROI.data;
+						unsigned char * previousPolarData = previousPolarROI.data;
+						widthStep = motionPolarROI.cols * motionPolarROI.elemSize();
+						for(int j=0; j<motionPolarROI.rows; ++j)
+						{
+							for(int i=0; i<motionPolarROI.cols; ++i)
+							{
+								float b = (float)motionPolarData[j*widthStep+i*3+0];
+								float g = (float)motionPolarData[j*widthStep+i*3+1];
+								float r = (float)motionPolarData[j*widthStep+i*3+2];
+								float previous_b = (float)previousPolarData[j*widthStep+i*3+0];
+								float previous_g = (float)previousPolarData[j*widthStep+i*3+1];
+								float previous_r = (float)previousPolarData[j*widthStep+i*3+2];
+
+								if(!(fabs(b-previous_b)/256.0f>=ratio || fabs(g-previous_g)/256.0f >= ratio || fabs(r-previous_r)/256.0f >= ratio))
+								{
+									motionPolarData[j*widthStep+i*3+0] = 0;
+									motionPolarData[j*widthStep+i*3+1] = 0;
+									motionPolarData[j*widthStep+i*3+2] = 0;
+								}
+							}
+						}
+
+						if(rosPublisherLocalPolar.getNumSubscribers())
+						{
+							cv_bridge::CvImage img;
+							img.header.frame_id = msg->header.frame_id;
+							img.header.stamp = msg->header.stamp;
+							img.encoding = sensor_msgs::image_encodings::BGR8;
+							img.image = polarROI;
+							rosPublisherLocalPolar.publish(img.toImageMsg());
+						}
+
+						if(rosPublisherLocalPolarReconstructed.getNumSubscribers())
+						{
+							cv::Mat reconstructedROI(imageROI.rows, imageROI.cols, imageROI.type());
+							IplImage iplPolarROI = polarROI;
+							IplImage iplReconstructedROI = reconstructedROI;
+							cvLogPolar( &iplPolarROI, &iplReconstructedROI, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
+
+							cv_bridge::CvImage img;
+							img.header.frame_id = msg->header.frame_id;
+							img.header.stamp = msg->header.stamp;
+							img.encoding = sensor_msgs::image_encodings::BGR8;
+							img.image = reconstructedROI;
+							rosPublisherLocalPolarReconstructed.publish(img.toImageMsg());
+						}
+
+						if(rosPublisherMotionLocalPolar.getNumSubscribers())
+						{
+							cv_bridge::CvImage img;
+							img.header.frame_id = msg->header.frame_id;
+							img.header.stamp = msg->header.stamp;
+							img.encoding = sensor_msgs::image_encodings::BGR8;
+							img.image = motionPolarROI;
+							rosPublisherMotionLocalPolar.publish(img.toImageMsg());
+						}
+
+						//reconstruct image
+						cv::Mat reconstructedROI(imageROI.rows, imageROI.cols, imageROI.type());
+						IplImage iplMotionPolarROI = motionPolarROI;
+						IplImage iplReconstructedROI = reconstructedROI;
+						cvLogPolar( &iplMotionPolarROI, &iplReconstructedROI, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
+
+						if(rosPublisherMotionLocalPolarReconstructed.getNumSubscribers())
+						{
+							cv_bridge::CvImage img;
+							img.header.frame_id = msg->header.frame_id;
+							img.header.stamp = msg->header.stamp;
+							img.encoding = sensor_msgs::image_encodings::BGR8;
+							img.image = reconstructedROI;
+							rosPublisherMotionLocalPolarReconstructed.publish(img.toImageMsg());
+						}
+
+						if(localAttention)
+						{
+							float distNearest = 0.0f;
+							unsigned char * reconstructedData = reconstructedROI.data;
+							widthStep = reconstructedROI.cols * reconstructedROI.elemSize();
+							for(int j=0; j<reconstructedROI.rows; ++j)
+							{
+								for(int i=0; i<reconstructedROI.cols; ++i)
+								{
+									if(reconstructedData[j*widthStep+i*3+0] ||
+										reconstructedData[j*widthStep+i*3+1] ||
+										reconstructedData[j*widthStep+i*3+2])
+									{
+										float dist = std::sqrt(float((i-centerROILocal.x)*(i-centerROILocal.x) + (j-centerROILocal.y)*(j-centerROILocal.y)));
+										if((nearestMovingPixel.x == 0 && nearestMovingPixel.y == 0) ||
+											dist < distNearest)
+										{
+											nearestMovingPixel.y = j;
+											nearestMovingPixel.x = i;
+											distNearest = dist;
+										}
+									}
+								}
+							}
+						}
+
+						// PLOT
+						if(g_curveR && g_curveG && g_curveB)
+						{
+							cv::Mat ref = ptr->image;
+							if(polarCheckBox->isChecked())
+							{
+								ref = reconstructedROI;
+							}
+
+							if(spinX->value()<0 || spinX->value() > ref.cols)
+							{
+								spinX->setValue(ref.cols/2);
+							}
+							if(spinY->value()<0 || spinY->value() > ref.rows)
+							{
+								spinY->setValue(ref.rows/2);
+							}
+
+							//take pixel (OpenCV is BGR)
+							float r = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 2);
+							float g = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 1);
+							float b = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 0);
+							if(g_curveR->itemsSize())
+							{
+								float lastR = g_curveR->getItemData(g_curveR->itemsSize()-1).y();
+								float lastG = g_curveG->getItemData(g_curveG->itemsSize()-1).y();
+								float lastB = g_curveB->getItemData(g_curveB->itemsSize()-1).y();
+								QMetaObject::invokeMethod(g_curveRRatio, "addValue", Q_ARG(float, fabs(r-lastR)/255.0f) );
+								QMetaObject::invokeMethod(g_curveGRatio, "addValue", Q_ARG(float, fabs(g-lastG)/255.0f) );
+								QMetaObject::invokeMethod(g_curveBRatio, "addValue", Q_ARG(float, fabs(b-lastB)/255.0f) );
+							}
+
+							QMetaObject::invokeMethod(g_curveR, "addValue", Q_ARG(float, (float)r));
+							QMetaObject::invokeMethod(g_curveG, "addValue", Q_ARG(float, (float)g));
+							QMetaObject::invokeMethod(g_curveB, "addValue", Q_ARG(float, (float)b));
+						}
+					}
+
+				}
+
+				if(!localAttention)
+				{
+					//global, may be outside of the ROI
+					float distNearest = 0.0f;
+					unsigned char * data = motion.data;
+					widthStep = motion.cols * motion.elemSize();
+					for(int j=0; j<motion.rows; ++j)
+					{
+						for(int i=0; i<motion.cols; ++i)
+						{
+							if(data[j*widthStep+i*3+0] ||
+									data[j*widthStep+i*3+1] ||
+									data[j*widthStep+i*3+2])
+							{
+								float dist = std::sqrt(float((i-centerROIGlobal.x)*(i-centerROIGlobal.x) + (j-centerROIGlobal.y)*(j-centerROIGlobal.y)));
+								if((nearestMovingPixel.x == 0 && nearestMovingPixel.y == 0) ||
+									dist < distNearest)
+								{
+									nearestMovingPixel.y = j;
+									nearestMovingPixel.x = i;
+									distNearest = dist;
+								}
+							}
+						}
+					}
+				}
+
+				if(!attentionDisabled && nearestMovingPixel.x && nearestMovingPixel.y)
+				{
+					if(localAttention)
+					{
+						ROS_INFO("nearestMovingPixel center(local:global)=(%d,%d:%d,%d) nearest(local:global)=(%d,%d:%d,%d)",
+								centerROILocal.x,
+								centerROILocal.y,
+								centerROILocal.x+roi.x,
+								centerROILocal.y+roi.y,
+								nearestMovingPixel.x,
+								nearestMovingPixel.y,
+								nearestMovingPixel.x+roi.x,
+								nearestMovingPixel.y+roi.y);
+						nearestMovingPixel.x += roi.x;
+						nearestMovingPixel.y += roi.y;
+						if( nearestMovingPixel.x > roi.x &&
+							nearestMovingPixel.x<roi.x+roi.width &&
+							nearestMovingPixel.x>roi.width/2 &&
+							nearestMovingPixel.x<ptr->image.cols - roi.width/2)
+						{
+							roi.x = nearestMovingPixel.x-roi.width/2;
+						}
+						if( nearestMovingPixel.y>roi.y &&
+							nearestMovingPixel.y<roi.y+roi.height &&
+							nearestMovingPixel.y>roi.height/2 &&
+							nearestMovingPixel.y<ptr->image.rows - roi.height/2)
+						{
+							roi.y = nearestMovingPixel.y-roi.height/2;
+						}
+					}
+					else
+					{
+						ROS_INFO("nearestMovingPixel center(global)=(%d,%d) nearest(global)=(%d,%d)",
+								centerROIGlobal.x,
+								centerROIGlobal.y,
+								nearestMovingPixel.x,
+								nearestMovingPixel.y);
+						if(	nearestMovingPixel.x>roi.width/2 &&
+							nearestMovingPixel.x<ptr->image.cols - roi.width/2)
+						{
+							roi.x = nearestMovingPixel.x-roi.width/2;
+						}
+						else if(nearestMovingPixel.x>=ptr->image.cols - roi.width/2)
+						{
+							roi.x = ptr->image.cols - roi.width - 1;
+						}
+						else
+						{
+							roi.x =0;
+						}
+						if(	nearestMovingPixel.y>roi.height/2 &&
+							nearestMovingPixel.y<ptr->image.rows - roi.height/2)
+						{
+							roi.y = nearestMovingPixel.y-roi.height/2;
+						}
+						else if(nearestMovingPixel.y>=ptr->image.rows - roi.height/2)
+						{
+							roi.y = ptr->image.rows - roi.height - 1;
+						}
+						else
+						{
+							roi.y = 0;
+						}
+					}
+				}
+				cv::Mat newImageROI = cv::Mat(ptr->image, roi);
+				int radius = newImageROI.cols/2;
+				float M = POLAR_RINGS/std::log(radius);
+				previousPolarROI = cv::Mat(POLAR_RAYS, POLAR_RINGS, CV_8UC3);
+				IplImage iplPolar = previousPolarROI;
+				IplImage iplImageROI = newImageROI;
+				cvLogPolar( &iplImageROI, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
+			}
+			else
+			{
+				int size = ptr->image.cols < ptr->image.rows?ptr->image.cols/ROI_RATIO: ptr->image.rows/ROI_RATIO;
+				int x = size<ptr->image.cols?(ptr->image.cols-size)/2:0;
+				int y = size<ptr->image.rows?(ptr->image.rows-size)/2:0;
+				roi = cv::Rect(x, y, size, size);
+			}
+			previousGlobalImage = ptr->image.clone();
+
+			ROS_INFO("polar ROI (%d,%d,%d,%d)", roi.x, roi.y, roi.width, roi.height);
+			cv::Mat localImage = cv::Mat(ptr->image, roi).clone();
+
+			/*int radius = polarImage.cols/2;
+			float M = 64/std::log(radius);
+			ROS_INFO("src size=(%d,%d) radius=%d, M=%f", polarImage.cols, polarImage.rows, radius, M);
+			cv::Mat tmpPolar(128, 64, CV_8UC3);
+			IplImage iplTmpPolar = tmpPolar;
+			IplImage iplPolar = polarImage;
+			cvLogPolar( &iplPolar, &iplTmpPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
+			cvLogPolar( &iplTmpPolar, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
+*/
+
+			if(rosPublisherLocal.getNumSubscribers())
+			{
+				cv_bridge::CvImage img;
+				img.header.frame_id = msg->header.frame_id;
+				img.header.stamp = msg->header.stamp;
+				img.encoding = sensor_msgs::image_encodings::BGR8;
+				img.image = localImage;
+				rosPublisherLocal.publish(img.toImageMsg());
+			}
+
+
+			if(rosPublisherMotionGlobal.getNumSubscribers())
+			{
+				cv_bridge::CvImage img;
+				img.header.frame_id = msg->header.frame_id;
+				img.header.stamp = msg->header.stamp;
+				cv::rectangle(motion, cv::Point2f(roi.x, roi.y), cv::Point2f(roi.x+roi.width, roi.y+roi.height), cv::Scalar(0, 255, 0), 1);
+				img.encoding = sensor_msgs::image_encodings::BGR8;
+				img.image = motion;
+				rosPublisherMotionGlobal.publish(img.toImageMsg());
+			}
+
+			if(rosPublisherWithRoi.getNumSubscribers())
+			{
+				cv::Mat imageWithRoi = ptr->image.clone();
+				cv::rectangle(imageWithRoi, cv::Point2f(roi.x, roi.y), cv::Point2f(roi.x+roi.width, roi.y+roi.height), cv::Scalar(0, 255, 0), 1);
+				cv_bridge::CvImage img;
+				img.header.frame_id = msg->header.frame_id;
+				img.header.stamp = msg->header.stamp;
+				img.encoding = sensor_msgs::image_encodings::BGR8;
+				img.image = imageWithRoi;
+				rosPublisherWithRoi.publish(img.toImageMsg());
+			}
+
+		}
+	}
+}
+
+void my_handler(int s){
+	QApplication::closeAllWindows();
+	QApplication::exit();
+}
+
+int main(int argc, char * argv[])
+{
+	ros::init(argc, argv, "visual_attention");
+
+	ros::NodeHandle n;
+	image_transport::ImageTransport it(n);
+	image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
+	rosPublisherMotionGlobal = it.advertise("image_motion", 1);
+	rosPublisherMotionLocal = it.advertise("image_motion_local", 1);
+	rosPublisherLocal = it.advertise("image_local", 1);
+	rosPublisherMotionLocalPolar = it.advertise("image_motion_local_polar", 1);
+	rosPublisherMotionLocalPolarReconstructed = it.advertise("image_motion_local_polar_reconstructed", 1);
+	rosPublisherLocalPolar = it.advertise("image_local_polar", 1);
+	rosPublisherLocalPolarReconstructed = it.advertise("image_local_polar_reconstructed", 1);
+	rosPublisherWithRoi = it.advertise("image_with_roi", 1);
+
+	bool show_gui = true;
+	if(show_gui)
+	{
+		QApplication app(argc, argv);
+
+		QWidget widget;
+		widget.setLayout(new QVBoxLayout());
+		spinX = new QSpinBox(&widget);
+		spinY = new QSpinBox(&widget);
+		polarCheckBox = new QCheckBox("Polar", &widget);
+		polarCheckBox->setChecked(false);
+		QHBoxLayout * hLayout = new QHBoxLayout();
+		hLayout->addWidget(spinX);
+		hLayout->addWidget(spinY);
+		hLayout->addWidget(polarCheckBox);
+		widget.layout()->addItem(hLayout);
+		UPlot * plot = new UPlot(&widget);
+		plot->setWindowTitle("Pixel value");
+		plot->keepAllData(false);
+		plot->setMaxVisibleItems(50);
+		plot->setXLabel("Time (s)");
+		g_curveR = plot->addCurve("R", Qt::red);
+		g_curveG = plot->addCurve("G", Qt::green);
+		g_curveB = plot->addCurve("B", Qt::blue);
+		plot->setMinimumSize(600, 400);
+		widget.layout()->addWidget(plot);
+		widget.show();
+
+		UPlot plotRatio;
+		plotRatio.setWindowTitle("Pixel ratio");
+		plotRatio.keepAllData(false);
+		plotRatio.setMaxVisibleItems(50);
+		plotRatio.setXLabel("Time (s)");
+		g_curveRRatio = plotRatio.addCurve("R", Qt::red);
+		g_curveGRatio = plotRatio.addCurve("G", Qt::green);
+		g_curveBRatio = plotRatio.addCurve("B", Qt::blue);
+		plotRatio.setMinimumSize(600, 400);
+		plotRatio.show();
+
+		// Catch ctrl-c to close the gui
+		// (Place this after QApplication's constructor)
+		struct sigaction sigIntHandler;
+		sigIntHandler.sa_handler = my_handler;
+		sigemptyset(&sigIntHandler.sa_mask);
+		sigIntHandler.sa_flags = 0;
+		sigaction(SIGINT, &sigIntHandler, NULL);
+
+		ros::AsyncSpinner spinner(4); // Use 4 threads
+		spinner.start();
+		app.exec();
+		spinner.stop();
+	}
+	else
+	{
+		ros::spin();
+	}
+
+	return 0;
+}