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/CMakeLists.txt

@@ -44,7 +44,7 @@ rosbuild_add_executable(core_node src/CoreNode.cpp src/CoreWrapper.cpp)
 rosbuild_add_executable(input_node src/InputNode.cpp)
 rosbuild_add_executable(output_node src/OutputNode.cpp)
 rosbuild_add_executable(abtr_velocity_node src/AbtrVelocityNode.cpp)
-rosbuild_add_executable(image_to_sms_node src/ImageToSensoryMotorStateNode.cpp)
+rosbuild_add_executable(image_to_sms_node src/ImageToSensorimotorStateNode.cpp)
 FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui)
 IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
     INCLUDE(${QT_USE_FILE})

rtabmap/launch/all_tr_learning.launch

@@ -17,14 +17,6 @@
 	<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
 	
 	<!-- CAMERA -->
-	<node pkg="omni_camera_capture" type="omni_publisher" 
+	<include file="$(find rtabmap)/launch/camera.launch"/>
-		name="omni_camera" respawn="false">
-		<param name="device" value="/dev/video0"/>
-		<param name="refresh_rate" value="2"/>
-		<param name="frame_id" value="omni_camera_link" />
-		<param name="image_encoding" value="RGB" /> <!-- GREY or RGB-->
-		<param name="image_size" value="FULL" /> <!-- PARTIAL or FULL -->
-		<param name="scale_divisor" value="1" /> <!-- 1, 2, 3, 4 or 6  in PARTIAL size-->
-	</node>
 
 </launch>

rtabmap/launch/all_tr_learning_omni.launch

@@ -0,0 +1,31 @@
+<launch>
+
+	<!-- BRING UP TELEROBOT -->
+	<!-- BUS CAN MANAGER -->
+	<group ns="tr">
+        <param name="driver_type" value="TRCANDriver"/>
+        <node name="can_manager" type="can_manager" pkg="can_manager"
+                args="tr">
+        <remap from="/tr/tr/cmd_vel" to="/cmd_vel" />
+        </node>
+    </group>
+
+	<!-- TELEOP -->
+	<include file="$(find rtabmap)/launch/teleop.launch"/>
+	
+	<!-- RTAB-MAP -->
+	<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
+	
+	<!-- CAMERA -->
+	<node pkg="omni_camera_capture" type="omni_publisher" 
+		name="omni_camera" respawn="false">
+		<param name="device" value="/dev/video0"/>
+		<param name="refresh_rate" value="2"/>
+		<param name="frame_id" value="omni_camera_link" />
+		<param name="image_encoding" value="RGB" /> <!-- GREY or RGB-->
+		<param name="image_size" value="FULL" /> <!-- PARTIAL or FULL -->
+		<param name="scale_divisor" value="1" /> <!-- 1, 2, 3, 4 or 6  in PARTIAL size-->
+		<remap from="/image" to="/image_raw" />
+	</node>
+
+</launch>

rtabmap/launch/camera.launch

@@ -1,10 +1,10 @@
 <launch>
-	<!-- Camera parameters -->
+	<node pkg="uvc_camera" type="camera_node" name="uvc_camera" output="screen">
-	<param name="cam/device_id" value="0" type="int"/>
+	  <param name="width" type="int" value="640" />
-	<param name="cam/image_rate" value="1" type="int"/>
+	  <param name="height" type="int" value="480" />
-	<param name="cam/image_width" value="640" type="int"/>
+	  <param name="fps" type="int" value="5" />
-	<param name="cam/image_height" value="480" type="int"/>
+	  <param name="frame" type="string" value="wide_stereo" />
-	
+	  <param name="device" type="string" value="/dev/video0" />
-	<!-- Nodes -->
+	  <!-- <param name="camera_info_url" type="string" value="file://$(find uvc_camera)/example.yaml" /> -->
-	<node name="camera" pkg="rtabmap" type="camera_node"/>
+	</node>
 </launch>

rtabmap/launch/cameraOpenCV.launch

@@ -0,0 +1,10 @@
+<launch>
+	<!-- Camera parameters -->
+	<param name="cam/device_id" value="0" type="int"/>
+	<param name="cam/image_rate" value="1" type="int"/>
+	<param name="cam/image_width" value="640" type="int"/>
+	<param name="cam/image_height" value="480" type="int"/>
+	
+	<!-- Nodes -->
+	<node name="camera" pkg="rtabmap" type="camera_node"/>
+</launch>

rtabmap/launch/rtabmap_lc.launch

@@ -1,14 +1,14 @@
 <launch>
     <!-- RTAB-MAP LOOP CLOSURE DETECTION VERSION -->
-	<!-- Camera parameters -->
-	<param name="device_id" value="0" type="int"/>
-	<param name="image_rate" value="1" type="int"/>
-	<param name="image_width" value="640" type="int"/>
-	<param name="image_height" value="480" type="int"/>
-
 	<!-- Nodes -->
 	<node name="rtabmap_core" pkg="rtabmap" type="core_node"/>
 	<node name="rtabmap_gui" pkg="rtabmap" type="gui_node"/>
-	<node name="image_to_sms" pkg="rtabmap" type="image_to_sms_node"/>
+	<node name="image_to_sms" pkg="rtabmap" type="image_to_sms_node">
-	<node name="camera" pkg="rtabmap" type="camera_node"/>
+		<param name="image_rate" value="1" type="int"/>
+		<param name="resize_image_width" value="640" type="int"/>
+		<param name="resize_image_height" value="480" type="int"/>
+	</node>
+	
+	<!-- CAMERA -->
+	<include file="$(find rtabmap)/launch/camera.launch"/>
 </launch>

rtabmap/launch/rtabmap_sm.launch

@@ -2,10 +2,6 @@
 	<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
 	<!-- WARNING : Database is automatically deleted on each startup -->
 	<!--           See "delete_db_on_start" option below... -->
-
-	<!-- image_to_sm parameters -->
-	<param name="resize_image_width" value="640" type="int"/>
-	<param name="resize_image_height" value="480" type="int"/>
 	
 	<!-- Nodes -->	
 	<!-- cmd_vel_a has priority on cmd_vel_b -->
@@ -28,13 +24,16 @@
 	</node>
 
 	<node name="rtabmap_input" pkg="rtabmap" type="input_node">
-		<remap from="cmd_vel" to="cmd_vel" />
+		<remap from="/cmd_vel" to="/cmd_vel" />
-		<remap from="sm_state" to="sm_state" />
+		<remap from="/sm_state" to="/sm_state" />
-		<remap from="camera_data" to="camera_data" />
+		<remap from="/camera_data" to="/camera_data" />
 	</node>
 	
 	<node name="image_to_sms" pkg="rtabmap" type="image_to_sms_node">
-		<remap from="image" to="image" />
+		<param name="image_rate" value="1" type="int"/>
-		<remap from="sm_state" to="camera_data" />
+		<param name="resize_image_width" value="640" type="int"/>
+		<param name="resize_image_height" value="480" type="int"/>
+		<remap from="/image_raw" to="/image_raw" />
+		<remap from="/sm_state" to="/camera_data" />
 	</node>
 </launch>

rtabmap/launch/testCamera.launch

@@ -1,11 +1,13 @@
 <launch>
-	<!-- Camera parameters -->
-	<param name="cam/device_id" value="0" type="int"/>
-	<param name="cam/image_rate" value="10" type="int"/>
-	<param name="cam/image_width" value="640" type="int"/>
-	<param name="cam/image_height" value="480" type="int"/>
-	
 	<!-- Nodes -->
 	<node name="cameraReceiver" pkg="rtabmap" type="camera_node_receiver" output="screen"/>
-	<node name="camera" pkg="rtabmap" type="camera_node"/>
+	
+	<node pkg="uvc_camera" type="camera_node" name="uvc_camera" output="screen">
+	  <param name="width" type="int" value="640" />
+	  <param name="height" type="int" value="480" />
+	  <param name="fps" type="int" value="30" />
+	  <param name="frame" type="string" value="wide_stereo" />
+	  <param name="device" type="string" value="/dev/video0" />
+	  <!-- <param name="camera_info_url" type="string" value="file://$(find uvc_camera)/example.yaml" /> -->
+	</node>
 </launch>

rtabmap/manifest.xml

@@ -14,6 +14,7 @@
   <depend package="sensor_msgs"/>
   <depend package="std_srvs"/>
   <depend package="opencv2"/>
+  <depend package="uvc_camera"/>
   <depend package="rtabmap_lib"/>
   <depend package="turtlesim"/>
 

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");
+	if(stateUpdated && actionsUpdated)
-	std::vector<float> tmp;
-
-	int sizeActions = -1;
-	commandMutex.lock();
 	{
+		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();
 
-	}
+		state->actuators = actions;
-	commandMutex.unlock();
 
-	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);
+	stateUpdated = true;
-	ROS_INFO("Sensorimotor state sent (sizeActions=%d)", sizeActions);
+	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);
+		ROS_WARN("Too many commands (%zu) > 10, removing the oldest...", commands.size());
-		v[0] = msg->linear.x;
+		//remove the oldest
-		v[1] = msg->linear.y;
+		commands.pop_front();
-		v[2] = msg->linear.z;
+	}
-		v[3] = msg->angular.x;
-		v[4] = msg->angular.y;
-		v[5] = msg->angular.z;
 
-		commands.push_back(v);
+	if(commands.size() == 10)
-
+	{
-		// 10 Hz + 1 max
+		actionsUpdated = true;
-		while(commands.size() > 11)
+		publish();
-		{
-			//remove the oldest
-			commands.pop_front();
-		}
 	}
-	commandMutex.unlock();
 }
 
 int main(int argc, char** argv)
 {
 	ros::init(argc, argv, "input_node");
 	ros::NodeHandle n;
-	rosPublisher = n.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
+	rosPublisher = n.advertise<rtabmap::SensoryMotorState>("/sm_state", 1);
-	ros::Subscriber image_sub = n.subscribe("camera_data", 1, smReceivedCallback);
+	ros::Subscriber image_sub = n.subscribe("/camera_data", 1, smReceivedCallback);
-	ros::Subscriber velocity_sub = n.subscribe("cmd_vel", 1, velocityReceivedCallback);
+	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;
-		commands = msg->actuators;
+	commandIndex = 0;
-		commandSize = msg->actuatorStep;
+	ROS_INFO("Rtabmap's actions received, commandSize=%d", commandSize);
-		commandIndex = 0;
-		ROS_INFO("Rtabmap's actions received, commandSize=%d", commandSize);
-	}
-	commandMutex.unlock();
 }
 
 void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
 {
 	ROS_INFO("Rtabmap's actions received");
-	commandMutex.lock();
+	commands = msg->info.actuators;
-	{
+	commandSize = msg->info.actuatorStep;
-		commands = msg->info.actuators;
+	ROS_INFO("Rtabmap's actions received, commandSize=%d, id=%d", commandSize, msg->info.refId);
-		commandSize = msg->info.actuatorStep;
+	commandIndex = 0;
-		ROS_INFO("Rtabmap's actions received, commandSize=%d, id=%d", commandSize, msg->info.refId);
-		commandIndex = 0;
-	}
-	commandMutex.unlock();
 }
 
 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 &&
+			geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
-				commandSize==6 &&
+			vel->linear.x = commands[commandIndex++];
-				commandIndex + (commandSize-1) < (int)commands.size())
+			vel->linear.y = commands[commandIndex++];
-			{
+			vel->linear.z = commands[commandIndex++];
-				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
+			vel->angular.x = commands[commandIndex++];
-				vel->linear.x = commands[commandIndex++];
+			vel->angular.y = commands[commandIndex++];
-				vel->linear.y = commands[commandIndex++];
+			vel->angular.z = commands[commandIndex++];
-				vel->linear.z = commands[commandIndex++];
+			ROS_INFO("Publishing vel");
-				vel->angular.x = commands[commandIndex++];
+			rosPublisher.publish(vel);
-				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;
 }