Updated Camera usage:

-now using uvc_camera (OpenCV capture is no longer supported in ROS : https://code.ros.org/trac/ros-pkg/ticket/4980)
-Added image_rate parameter in imageToSensorimotorState

Fixed synchronization in InputNode, between image (1Hz) and actions (10Hz)
Updated launch files

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

rtabmap/src/AbtrVelocityNode.cpp

@@ -91,4 +91,6 @@ int main(int argc, char** argv)
 		ros::spinOnce();
 		loop_rate.sleep();
 	}
+
+	return 0;
 }

rtabmap/src/CameraNode.cpp

@@ -46,7 +46,7 @@ int main(int argc, char** argv)
 
 	if(!camera || (camera && !camera->init()))
 	{
-		ROS_ERROR("Cannot initiate the camera");
+		ROS_ERROR("Cannot initiate the camera. Verify if OpenCV is built with ffmpeg support.");
 	}
 	else
 	{
@@ -61,4 +61,6 @@ int main(int argc, char** argv)
 	{
 		delete camera;
 	}
+
+	return 0;
 }

rtabmap/src/CameraNodeReceiver.cpp

@@ -42,11 +42,13 @@ int main(int argc, char** argv)
 
 	ros::init(argc, argv, "camera_node_receiver");
 	ros::NodeHandle n;
-	ros::Subscriber image_sub = n.subscribe("image", 1, imgReceivedCallback);
+	ros::Subscriber image_sub = n.subscribe("/image_raw", 1, imgReceivedCallback);
 
 	ROS_INFO("Waiting for images...");
 
 	ros::spin();
 
 	cvDestroyWindow("ImageReceived");
+
+	return 0;
 }

rtabmap/src/CameraNodeReceiverSM.cpp

@@ -35,11 +35,13 @@ int main(int argc, char** argv)
 
 	ros::init(argc, argv, "camera_node_receiver_sm");
 	ros::NodeHandle n;
-	ros::Subscriber image_sub = n.subscribe("sm_state", 1, smReceivedCallback);
+	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

@@ -21,7 +21,7 @@
 CameraWrapper::CameraWrapper() :
 	camera_(0)
 {
-	rosPublisher_ = nh_.advertise<sensor_msgs::Image>("image", 1);
+	rosPublisher_ = nh_.advertise<sensor_msgs::Image>("image_raw", 1);
 	changeCameraImgRateSrv_ = nh_.advertiseService("changeCameraImgRate", &CameraWrapper::changeCameraImgRateCallback, this);
 	UEventsManager::addHandler(this);
 }
@@ -118,7 +118,7 @@ void CameraWrapper::handleEvent(UEvent* anEvent)
 				sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(smState->getImage());
 				rosPublisher_.publish(msg);
 			}
-			catch (sensor_msgs::CvBridgeException ex)
+			catch (sensor_msgs::CvBridgeException & ex)
 			{
 				ROS_ERROR("%s", ex.what());
 			}

rtabmap/src/CoreNode.cpp

@@ -32,4 +32,6 @@ int main(int argc, char** argv)
 
 	ROS_INFO("RTAB-Map started...");
 	ros::spin();
+
+	return 0;
 }

rtabmap/src/ImageToSensorimotorStateNode.cpp

@@ -0,0 +1,176 @@
+/*
+ * 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;
+int imgRate = 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/double(imgRate);
+		period -= 0.01 * period; // 1% error
+	}
+	double elapsed = timer.getElapsedTime();
+	if(imgRate == 0 || elapsed > period)
+	{
+		timer.start();
+
+		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 processTimer;
+				rtabmap::SMState * smState = kpThreatment.process(image);
+				double processTime = processTimer.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;
+					}
+
+					ROS_INFO("Publishing smState (processing time=%fs, period=%fs, %f Hz)...", processTime, elapsed, elapsed>0?1/elapsed:0);
+					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_node");
+	//ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+
+	ros::NodeHandle nh("~");
+
+	nh.param("image_rate", imgRate, imgRate);
+	nh.param("resize_image_width", imgWidth, imgWidth);
+	nh.param("resize_image_height", imgHeight, imgHeight);
+
+	ROS_INFO("imgRate=%d\nresize_image_width=%d\nresize_image_height=%d", imgRate, imgWidth, imgHeight);
+
+	ros::Subscriber parametersUpdatedTopic = nh.subscribe("/parameters_updated", 1, parametersUpdatedCallback);
+	ros::Subscriber parametersLoadedTopic = nh.subscribe("/parameters_loaded", 1, parametersLoadedCallback);
+	rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("/sm_state", 1);
+
+	ros::Subscriber image_sub = nh.subscribe("/image_raw", 1, imgReceivedCallback);
+
+	updateParameters();
+
+	timer.start();
+
+	ros::spin();
+
+	return 0;
+}

rtabmap/src/ImageToSensoryMotorStateNode.cpp

@@ -1,149 +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/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

@@ -8,39 +8,53 @@
 #include "rtabmap/SensoryMotorState.h"
 #include <geometry_msgs/Twist.h>
 #include <utilite/UMutex.h>
+#include <utilite/UTimer.h>
 
 UMutex commandMutex;
 std::list<std::vector<float> > commands;
 ros::Publisher rosPublisher;
+UTimer timer;
+rtabmap::SensoryMotorStatePtr state;
 
-void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
+bool stateUpdated = false;
+bool actionsUpdated = false;
+
+void publish()
 {
-	ROS_INFO("Received camera data");
-	std::vector<float> tmp;
-
-	int sizeActions = -1;
-	commandMutex.lock();
+	if(stateUpdated && actionsUpdated)
 	{
+		std::vector<float> actions;
+		int sizeActions = -1;
 		for(std::list<std::vector<float> >::iterator iter = commands.begin(); iter!=commands.end();++iter)
 		{
-			tmp.insert(tmp.end(), iter->begin(), iter->end());
+			actions.insert(actions.end(), iter->begin(), iter->end());
 		}
 		sizeActions = commands.size();
 		commands.clear();
 
-	}
-	commandMutex.unlock();
+		state->actuators = actions;
 
-	rtabmap::SensoryMotorStatePtr state(new rtabmap::SensoryMotorState);
+		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;
+	}
+}
+
+void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
+{
+	ROS_INFO("Received camera data");
+
+	state = rtabmap::SensoryMotorStatePtr(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);
+	stateUpdated = true;
+	publish();
 }
 
 void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
@@ -52,35 +66,43 @@ void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
 			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 max
+	while(commands.size() > 10)
 	{
-		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;
+		ROS_WARN("Too many commands (%zu) > 10, removing the oldest...", commands.size());
+		//remove the oldest
+		commands.pop_front();
+	}
 
-		commands.push_back(v);
-
-		// 10 Hz + 1 max
-		while(commands.size() > 11)
-		{
-			//remove the oldest
-			commands.pop_front();
-		}
+	if(commands.size() == 10)
+	{
+		actionsUpdated = true;
+		publish();
 	}
-	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);
+	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);
+
+	timer.start();
 
 	ros::spin();
+
+	return 0;
 }

rtabmap/src/OutputNode.cpp

@@ -10,7 +10,6 @@
 #include <geometry_msgs/Twist.h>
 #include <utilite/UMutex.h>
 
-UMutex commandMutex;
 std::vector<float> commands;
 int commandSize = 0;
 int commandIndex = 0;
@@ -18,27 +17,19 @@ 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();
+	commands = msg->actuators;
+	commandSize = msg->actuatorStep;
+	commandIndex = 0;
+	ROS_INFO("Rtabmap's actions received, commandSize=%d", commandSize);
 }
 
 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();
+	commands = msg->info.actuators;
+	commandSize = msg->info.actuatorStep;
+	ROS_INFO("Rtabmap's actions received, commandSize=%d, id=%d", commandSize, msg->info.refId);
+	commandIndex = 0;
 }
 
 int main(int argc, char** argv)
@@ -54,26 +45,23 @@ int main(int argc, char** argv)
 	ros::Rate loop_rate(10); // 10 Hz
 	while(ros::ok())
 	{
-		commandMutex.lock();
+		if(commandIndex>=0 &&
+			commandSize==6 &&
+			commandIndex + (commandSize-1) < (int)commands.size())
 		{
-			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);
-			}
+			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();
 	}
+	return 0;
 }