Added rtabmap_ros/RGBDImage and rtabmap_ros/UserData topics. Refactored input messages synchronization of rtabmap and rtabmapviz nodes. "depth_cameras" parameter is replaced with "rgbd_cameras" parameter. Multiple camera is handled through rtabmap_ros/RGBDImage input topics (same for rgbd_odometry node).

CMakeLists.txt

@@ -72,6 +72,8 @@ add_message_files(
    Point2f.msg
    Point3f.msg
    Goal.msg
+   RGBDImage.msg
+   UserData.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -162,9 +164,15 @@ SET(rtabmap_ros_lib_src
    src/nodelets/obstacles_detection.cpp
    src/nodelets/obstacles_detection_old.cpp
    src/nodelets/point_cloud_aggregator.cpp
+   src/nodelets/rgbd_sync.cpp
    src/OdometryROS.cpp
    src/MsgConversion.cpp
    src/MapsManager.cpp
+   src/CommonDataSubscriber.cpp
+   src/impl/CommonDataSubscriberDepth.cpp
+   src/impl/CommonDataSubscriberStereo.cpp
+   src/impl/CommonDataSubscriberRGBD.cpp
+   src/impl/CommonDataSubscriberRGBD2.cpp
 )   
 
 # If costmap_2d is found, add the plugin
@@ -327,6 +335,7 @@ install(TARGETS
    rtabmap 
    rtabmapviz 
    rgbd_odometry 
+   rgbdicp_odometry 
    stereo_odometry
    map_assembler
    map_optimizer
@@ -341,6 +350,7 @@ install(TARGETS
    rtabmap_ros 
    rtabmap 
    rgbd_odometry 
+   rgbdicp_odometry 
    stereo_odometry
    map_assembler
    map_optimizer

include/rtabmap_ros/CommonDataSubscriber.h

@@ -0,0 +1,244 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
+#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <nav_msgs/Odometry.h>
+
+#include <rtabmap_ros/RGBDImage.h>
+#include <rtabmap_ros/UserData.h>
+#include <rtabmap_ros/OdomInfo.h>
+#include <rtabmap_ros/CommonDataSubscriberDefines.h>
+
+namespace rtabmap_ros {
+
+class CommonDataSubscriber {
+public:
+	CommonDataSubscriber();
+	virtual ~CommonDataSubscriber();
+
+	bool isSubscribedToDepth() const  {return subscribedToDepth_;}
+	bool isSubscribedToStereo() const {return subscribedToStereo_;}
+	bool isSubscribedToRGBD() const   {return subscribedToRGBD_;}
+	bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
+	bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
+	bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
+	bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD();}
+	int getQueueSize() const {return queueSize_;}
+
+protected:
+	virtual void commonDepthCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const rtabmap_ros::UserDataConstPtr & userDataMsg,
+				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+				const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
+	virtual void commonStereoCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const sensor_msgs::ImageConstPtr& leftImageMsg,
+				const sensor_msgs::ImageConstPtr& rightImageMsg,
+				const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+				const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
+
+	void commonSingleDepthCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const rtabmap_ros::UserDataConstPtr & userDataMsg,
+				const cv_bridge::CvImageConstPtr & imageMsg,
+				const cv_bridge::CvImageConstPtr & depthMsg,
+				const sensor_msgs::CameraInfo & cameraInfoMsg,
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+
+private:
+	void setupDepthCallbacks(
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+	void setupStereoCallbacks(
+			bool subscribeOdom,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+	void setupRGBDCallbacks(
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+	void setupRGBD2Callbacks(
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+
+private:
+	int queueSize_;
+	bool subscribedToDepth_;
+	bool subscribedToStereo_;
+	bool subscribedToRGBD_;
+	bool subscribedToScan2d_;
+	bool subscribedToScan3d_;
+	bool subscribedToOdomInfo_;
+
+	//for depth callback
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	//for rgbd callback
+	ros::Subscriber rgbdSub_;
+	std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
+
+	//stereo callback
+	image_transport::SubscriberFilter imageRectLeft_;
+	image_transport::SubscriberFilter imageRectRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
+	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
+	message_filters::Subscriber<rtabmap_ros::UserData> userDataSub_;
+	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
+	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
+
+	// RGB + Depth
+	DATA_SYNCS3(depth, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
+	DATA_SYNCS4(depthScan2d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
+	DATA_SYNCS4(depthScan3d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(depthInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// RGB + Depth + Odom
+	DATA_SYNCS4(depthOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
+	DATA_SYNCS5(depthOdomScan2d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
+	DATA_SYNCS5(depthOdomScan3d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(depthOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// RGB + Depth + User Data
+	DATA_SYNCS4(depthData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
+	DATA_SYNCS5(depthDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
+	DATA_SYNCS5(depthDataScan3d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(depthDataInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// RGB + Depth + Odom + User Data
+	DATA_SYNCS5(depthOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
+	DATA_SYNCS6(depthOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
+	DATA_SYNCS6(depthOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS6(depthOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// Stereo
+	DATA_SYNCS4(stereo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo);
+	DATA_SYNCS5(stereoInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// Stereo + Odom
+	DATA_SYNCS5(stereoOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo);
+	DATA_SYNCS6(stereoOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// 1 RGBD
+	void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr&);
+	DATA_SYNCS2(rgbdScan2d, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS2(rgbdScan3d, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS2(rgbdInfo, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 1 RGBD + Odom
+	DATA_SYNCS2(rgbdOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImage);
+	DATA_SYNCS3(rgbdOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbdOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 1 RGBD + User Data
+	DATA_SYNCS2(rgbdData, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
+	DATA_SYNCS3(rgbdDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS3(rgbdDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbdDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 1 RGBD + Odom + User Data
+	DATA_SYNCS3(rgbdOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
+	DATA_SYNCS4(rgbdOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 2 RGBD
+	DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS3(rgbd2Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS3(rgbd2Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbd2Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 2 RGBD + Odom
+	DATA_SYNCS3(rgbd2Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS4(rgbd2OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 2 RGBD + User Data
+	DATA_SYNCS3(rgbd2Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS4(rgbd2DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS4(rgbd2DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbd2DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 2 RGBD + Odom + User Data
+	DATA_SYNCS4(rgbd2OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS5(rgbd2OdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+};
+
+} /* namespace rtabmap_ros */
+
+#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ */

include/rtabmap_ros/CommonDataSubscriberDefines.h

@@ -0,0 +1,194 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
+#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
+
+
+#define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
+	typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \
+	message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS2(PREFIX, MSG0, MSG1) \
+		DATA_SYNC2(PREFIX, Approximate, MSG0, MSG1) \
+		DATA_SYNC2(PREFIX, Exact, MSG0, MSG1) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&);
+
+#define DATA_SYNC3(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2) \
+		typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2> PREFIX##SYNC_NAME##SyncPolicy; \
+		message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS3(PREFIX, MSG0, MSG1, MSG2) \
+		DATA_SYNC3(PREFIX, Approximate, MSG0, MSG1, MSG2) \
+		DATA_SYNC3(PREFIX, Exact, MSG0, MSG1, MSG2) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&); \
+
+#define DATA_SYNC4(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3) \
+		typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3> PREFIX##SYNC_NAME##SyncPolicy; \
+		message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS4(PREFIX, MSG0, MSG1, MSG2, MSG3) \
+		DATA_SYNC4(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3) \
+		DATA_SYNC4(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&); \
+
+#define DATA_SYNC5(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4) \
+		typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4> PREFIX##SYNC_NAME##SyncPolicy; \
+		message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS5(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4) \
+		DATA_SYNC5(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4) \
+		DATA_SYNC5(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&); \
+
+#define DATA_SYNC6(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
+		typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5> PREFIX##SYNC_NAME##SyncPolicy; \
+		message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS6(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
+		DATA_SYNC6(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
+		DATA_SYNC6(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&, const MSG5##ConstPtr&); \
+
+// Constructor
+#define SYNC_INIT(PREFIX) \
+	PREFIX##ApproximateSync_(0), \
+	PREFIX##ExactSync_(0)
+
+// Destructor
+#define SYNC_DEL(PREFIX) \
+	if(PREFIX##ApproximateSync_) delete PREFIX##ApproximateSync_; \
+	if(PREFIX##ExactSync_) delete PREFIX##ExactSync_;
+
+// Sync declarations
+#define SYNC_DECL2(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \
+		if(APPROX) \
+		{ \
+			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
+					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
+		} \
+		else \
+		{ \
+			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
+					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
+		} \
+		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s", \
+				ros::this_node::getName().c_str(), \
+				APPROX?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str());
+
+#define SYNC_DECL3(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \
+		if(APPROX) \
+		{ \
+			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
+					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
+		} \
+		else \
+		{ \
+			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
+					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
+		} \
+		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s", \
+				ros::this_node::getName().c_str(), \
+				APPROX?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str(), \
+				SUB2.getTopic().c_str());
+
+#define SYNC_DECL4(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
+		if(APPROX) \
+		{ \
+			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
+					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
+		} \
+		else \
+		{ \
+			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
+					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
+		} \
+		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s", \
+				ros::this_node::getName().c_str(), \
+				APPROX?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str(), \
+				SUB2.getTopic().c_str(), \
+				SUB3.getTopic().c_str());
+
+#define SYNC_DECL5(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \
+		if(APPROX) \
+		{ \
+			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
+					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
+		} \
+		else \
+		{ \
+			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
+					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
+		} \
+		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
+				ros::this_node::getName().c_str(), \
+				approxSync?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str(), \
+				SUB2.getTopic().c_str(), \
+				SUB3.getTopic().c_str(), \
+				SUB4.getTopic().c_str());
+
+#define SYNC_DECL6(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \
+		if(APPROX) \
+		{ \
+			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
+					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
+		} \
+		else \
+		{ \
+			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
+					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
+		} \
+		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
+				ros::this_node::getName().c_str(), \
+				APPROX?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str(), \
+				SUB2.getTopic().c_str(), \
+				SUB3.getTopic().c_str(), \
+				SUB4.getTopic().c_str(), \
+				SUB5.getTopic().c_str());
+
+
+#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ */

include/rtabmap_ros/CoreWrapper.h

@@ -38,11 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <sensor_msgs/LaserScan.h>
-#include <nav_msgs/Odometry.h>
 #include <nav_msgs/GetMap.h>
 
 #include <rtabmap/core/Parameters.h>
@@ -54,17 +49,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/SetGoal.h"
 #include "rtabmap_ros/SetLabel.h"
 #include "rtabmap_ros/Goal.h"
+#include "rtabmap_ros/CommonDataSubscriber.h"
 
 #include "MapsManager.h"
 
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/sync_policies/exact_time.h>
-
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
-
 #ifdef WITH_OCTOMAP_ROS
 #include <octomap_msgs/GetOctomap.h>
 #endif
@@ -81,128 +69,48 @@ namespace rtabmap {
 class StereoDense;
 }
 
-class CoreWrapper
+namespace rtabmap_ros {
+
+class CoreWrapper : public CommonDataSubscriber
 {
 public:
 	CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters);
 	virtual ~CoreWrapper();
 
 private:
-	void setupCallbacks(
-			bool subscribeDepth,
-			bool subscribeScan2d,
-			bool subscribeScan3d,
-			bool subscribeStereo,
-			int queueSize,
-			bool approxSync,
-			int depthCameras);
-	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
 
-	bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
-	bool commonOdomTFUpdate(const ros::Time & stamp); // TF odom
+	bool odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
+	bool odomTFUpdate(const ros::Time & stamp); // TF odom
 
-	void commonDepthCallback(
-				const std::string & odomFrameId,
-				const sensor_msgs::ImageConstPtr& imageMsg,
-				const sensor_msgs::ImageConstPtr& depthMsg,
-				const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+	virtual void commonDepthCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const rtabmap_ros::UserDataConstPtr & userDataMsg,
+				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+				const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
 				const sensor_msgs::LaserScanConstPtr& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
-	void commonDepthCallback(
-				const std::string & odomFrameId,
-				const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
-				const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
-				const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
-	void commonStereoCallback(
-				const std::string & odomFrameId,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+	void commonDepthCallbackImpl(
+			const std::string & odomFrameId,
+			const rtabmap_ros::UserDataConstPtr & userDataMsg,
+			const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+			const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+			const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
+			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+	virtual void commonStereoCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
 				const sensor_msgs::ImageConstPtr& leftImageMsg,
 				const sensor_msgs::ImageConstPtr& rightImageMsg,
 				const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 				const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 				const sensor_msgs::LaserScanConstPtr& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
 
-	// with odom msg
-	void depthCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScanCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg);
-	void depthScan3dCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
-	void stereoCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg);
-	void stereoScanCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg);
-	void stereoScan3dCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg);
-	void depth2Callback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& image1Msg,
-			const sensor_msgs::ImageConstPtr& imageDepth1Msg,
-			const sensor_msgs::CameraInfoConstPtr& camInfo1Msg,
-			const sensor_msgs::ImageConstPtr& image2Msg,
-			const sensor_msgs::ImageConstPtr& imageDept2hMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfo2Msg);
-
-	// without odom, when TF is used for odom
-	void depthTFCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScanTFCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg);
-	void depthScan3dTFCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
-	void stereoTFCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg);
-	void stereoScanTFCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg);
-	void stereoScan3dTFCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
+	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
 
 	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
@@ -269,8 +177,8 @@ private:
 	rtabmap::ParametersMap parameters_;
 
 	std::string frameId_;
-	std::string mapFrameId_;
 	std::string odomFrameId_;
+	std::string mapFrameId_;
 	std::string groundTruthFrameId_;
 	std::string groundTruthBaseFrameId_;
 	std::string configPath_;
@@ -302,179 +210,6 @@ private:
 	ros::Publisher globalPathPub_;
 	ros::Publisher localPathPub_;
 
-	// for loop closure detection only
-	image_transport::Subscriber defaultSub_;
-
-	//for depth callback
-	std::vector<image_transport::SubscriberFilter*> imageSubs_;
-	std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
-	std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>*> cameraInfoSubs_;
-
-	//stereo callback
-	image_transport::SubscriberFilter imageRectLeft_;
-	image_transport::SubscriberFilter imageRectRight_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
-
-	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
-	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
-	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthScanExactSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanExactSyncPolicy> * depthScanExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2> MyDepthScan3dExactSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dExactSyncPolicy> * depthScan3dExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
-	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthExactSyncPolicy;
-	message_filters::Synchronizer<MyDepthExactSyncPolicy> * depthExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry> MyStereoScanSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry> MyStereoScan3dSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			nav_msgs::Odometry> MyStereoApproxSyncPolicy;
-	message_filters::Synchronizer<MyStereoApproxSyncPolicy> * stereoApproxSync_;
-
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			nav_msgs::Odometry> MyStereoExactSyncPolicy;
-	message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
-	message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
-
-	// without odom, when TF is used for odom
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthScanTFExactSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanTFExactSyncPolicy> * depthScanTFExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2> MyDepthScan3dTFExactSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dTFExactSyncPolicy> * depthScan3dTFExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
-	typedef message_filters::sync_policies::ExactTime<
-				sensor_msgs::Image,
-				sensor_msgs::Image,
-				sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy;
-	message_filters::Synchronizer<MyDepthTFExactSyncPolicy> * depthTFExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2> MyStereoScan3dTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoApproxTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoApproxTFSyncPolicy> * stereoApproxTFSync_;
-
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoExactTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoExactTFSyncPolicy> * stereoExactTFSync_;
-
 	tf2_ros::TransformBroadcaster tfBroadcaster_;
 	tf::TransformListener tfListener_;
 
@@ -508,6 +243,9 @@ private:
 
 	boost::thread* transformThread_;
 
+	// for loop closure detection only
+	image_transport::Subscriber defaultSub_;
+
 	bool stereoToDepth_;
 	bool odomSensorSync_;
 	float rate_;
@@ -516,5 +254,7 @@ private:
 	ros::Time previousStamp_;
 };
 
+}
+
 #endif /* COREWRAPPER_H_ */
 

include/rtabmap_ros/GuiWrapper.h

@@ -39,21 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tf/transform_listener.h>
 
 #include <geometry_msgs/TwistStamped.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <sensor_msgs/LaserScan.h>
-#include <nav_msgs/Odometry.h>
 #include <nav_msgs/Path.h>
 #include <std_msgs/Bool.h>
 
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/sync_policies/exact_time.h>
-
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
+#include <rtabmap_ros/CommonDataSubscriber.h>
 
 namespace rtabmap
 {
@@ -62,7 +51,9 @@ namespace rtabmap
 
 class QApplication;
 
-class GuiWrapper : public UEventsHandler
+namespace rtabmap_ros {
+
+class GuiWrapper : public UEventsHandler, public CommonDataSubscriber
 {
 public:
 	GuiWrapper(int & argc, char** argv);
@@ -76,32 +67,16 @@ private:
 	void goalPathCallback(const rtabmap_ros::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg);
 	void goalReachedCallback(const std_msgs::BoolConstPtr & value);
 
-	void setupCallbacks(
-			bool subscribeDepth,
-			bool subscribeLaserScan2d,
-			bool subscribeLaserScan3d,
-			bool subscribeOdomInfo,
-			bool subscribeStereo,
-			int queueSize,
-			int depthCameras);
-
-	void commonDepthCallback(
+	virtual void commonDepthCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+			const rtabmap_ros::UserDataConstPtr & userDataMsg,
+			const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+			const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+			const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
 			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
 			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
-	void commonDepthCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
-			const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
-			const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
-			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
-	void commonStereoCallback(
+	virtual void commonStereoCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
 			const sensor_msgs::ImageConstPtr& leftImageMsg,
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
@@ -113,150 +88,6 @@ private:
 
 	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
 
-	// With odom msg
-	void depthCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depth2Callback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& image1Msg,
-			const sensor_msgs::ImageConstPtr& depth1Msg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
-			const sensor_msgs::ImageConstPtr& image2Msg,
-			const sensor_msgs::ImageConstPtr& depth2Msg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
-	void depthOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
-	void depthOdomInfo2Callback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& image1Msg,
-			const sensor_msgs::ImageConstPtr& depth1Msg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
-			const sensor_msgs::ImageConstPtr& image2Msg,
-			const sensor_msgs::ImageConstPtr& depth2Msg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
-	void depthScanCallback(
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
-		    const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScanOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScan3dCallback(
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScan3dOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-
-	void stereoScanCallback(
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoScanOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoScan3dCallback(
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoScan3dOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-
-	// with TF
-	void depthTFCallback(const sensor_msgs::ImageConstPtr& imageMsg,
-					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
-					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthOdomInfoTFCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
-	void depthScanTFCallback(
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
-		    const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
-	void depthScan3dTFCallback(
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
-
-	void stereoScanTFCallback(
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoScan3dTFCallback(
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoOdomInfoTFCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoTFCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-
 	void processRequestedMap(const rtabmap_ros::MapData & map);
 
 private:
@@ -280,18 +111,6 @@ private:
 	ros::Subscriber goalReachedTopic_;
 
 	ros::Subscriber defaultSub_; // odometry only
-	std::vector<image_transport::SubscriberFilter*> imageSubs_;
-	std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
-	std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>* > cameraInfoSubs_;
-	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
-	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
-	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
-	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
-
-	image_transport::SubscriberFilter imageRectLeft_;
-	image_transport::SubscriberFilter imageRectRight_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
 
 	typedef message_filters::sync_policies::ExactTime<
 			rtabmap_ros::Info,
@@ -302,191 +121,8 @@ private:
 			rtabmap_ros::Goal,
 			nav_msgs::Path> MyGoalPathSyncPolicy;
 	message_filters::Synchronizer<MyGoalPathSyncPolicy> * goalPathSync_;
-
-	// with odom msg
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScanOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy> * depthScanOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScan3dOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dOdomInfoSyncPolicy> * depthScan3dOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
-	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoSyncPolicy;
-	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScanSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScanOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy> * stereoScanOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScan3dOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dOdomInfoSyncPolicy> * stereoScan3dOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
-	message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthOdomInfo2SyncPolicy;
-	message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy> * depthOdomInfo2Sync_;
-
-	// with odom TF
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::LaserScan,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::PointCloud2,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScan3dTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthOdomInfoTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy> * depthOdomInfoTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoTFSyncPolicy> * stereoTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::LaserScan,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::PointCloud2,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScan3dTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoOdomInfoTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy> * stereoOdomInfoTFSync_;
 };
 
+}
+
 #endif /* GUIWRAPPER_H_ */

include/rtabmap_ros/MsgConversion.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <cv_bridge/cv_bridge.h>
 
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>
@@ -55,6 +56,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/NodeData.h>
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap_ros/Info.h>
+#include <rtabmap_ros/RGBDImage.h>
+#include <rtabmap_ros/UserData.h>
 
 namespace rtabmap_ros {
 
@@ -67,6 +70,9 @@ rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg
 void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg);
 rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg);
 
+void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
+void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+
 // copy data
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
 cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy = true);
@@ -140,6 +146,9 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
 
+cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
+void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress);
+
 inline double timestampFromROS(const ros::Time & stamp) {return double(stamp.sec) + double(stamp.nsec)/1000000000.0;}
 
 // common stuff
@@ -162,9 +171,9 @@ rtabmap::Transform getTransform(
 		double waitForTransform);
 
 bool convertRGBDMsgs(
-		const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
-		const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
-		const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
+		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+		const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,

launch/demo/demo_two_kinects.launch

@@ -1,7 +1,7 @@
 
 <launch>
    
-  <!-- Multi-cameras demo  with 2 Kinects -->
+  <!-- Multi-cameras demo with 2 Kinects -->
    
   <!-- Cameras -->
   <include file="$(find freenect_launch)/launch/freenect.launch">
@@ -46,21 +46,32 @@
    <arg name="local_map"       default="1000" />
    <arg name="odom_info_data"  default="true" />
    <arg name="wait_for_transform"  default="true" />
+   
+   <!-- sync rgb/depth images per camera -->
+   <group ns="camera1">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera1_nodelet_manager">
+      <remap from="rgb/image"       to="rgb/image_rect_color"/>
+      <remap from="depth/image"     to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info" to="rgb/camera_info"/>
+    </node>
+   </group>
+   <group ns="camera2">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera2_nodelet_manager">
+      <remap from="rgb/image"       to="rgb/image_rect_color"/>
+      <remap from="depth/image"     to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info" to="rgb/camera_info"/>
+    </node>
+   </group>
         
   <group ns="rtabmap">
   
     <!-- Odometry -->
     <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
-      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
-      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
-      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
-      
-      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
-      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
-      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
+      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
 	  
 	  <param name="frame_id"                 type="string" value="base_link"/>
-	  <param name="depth_cameras"            type="int"    value="2"/>
+	  <param name="rgbd_cameras"             type="int"    value="2"/>
 	  <param name="wait_for_transform"       type="bool"   value="$(arg wait_for_transform)"/>
 	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
 	  <param name="Vis/FeatureType"         type="string" value="$(arg feature)"/>  
@@ -75,65 +86,37 @@
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
-      <param name="subscribe_depth"  type="bool"   value="true"/>
-      <param name="depth_cameras"    type="int"    value="2"/>
+      <param name="subscribe_depth"  type="bool"   value="false"/>
+      <param name="subscribe_rgbd"   type="bool"   value="true"/>
+      <param name="rgbd_cameras"    type="int"    value="2"/>
 	  <param name="frame_id"         type="string" value="base_link"/>
 	  <param name="gen_scan"         type="bool"   value="true"/>
 	  <param name="wait_for_transform" type="bool"   value="$(arg wait_for_transform)"/>
 	
-      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
-      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
-      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
-     
-      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
-      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
-      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
+      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
 
+      <param name="Grid/FromDepth"     type="string" value="false"/>
       <param name="Vis/MinInliers"     type="string" value="10"/>
       <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
     </node>
   
     <!-- Visualisation RTAB-Map -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
-      <param name="subscribe_depth"     type="bool"   value="true"/>
+      <param name="subscribe_depth"  type="bool"   value="false"/>
+      <param name="subscribe_rgbd"   type="bool"   value="true"/>
       <param name="subscribe_odom_info" type="bool"   value="$(arg odom_info_data)"/>
       <param name="frame_id"            type="string" value="base_link"/>
-      <param name="depth_cameras"       type="int"    value="2"/>
+      <param name="rgbd_cameras"       type="int"    value="2"/>
       <param name="wait_for_transform"  type="bool"   value="$(arg wait_for_transform)"/>
     
-      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
-      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
-      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
-     
-      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
-      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
-      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
+      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
     </node>
   
   </group>
   
   <!-- Visualization RVIZ -->
   <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
-  <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
-    <remap from="rgb/image_in"       to="camera1/rgb/image_rect_color"/>
-    <remap from="depth/image_in"     to="camera1/depth_registered/image_raw"/>
-    <remap from="rgb/camera_info_in" to="camera1/rgb/camera_info"/>
-    <remap from="odom_in"            to="rtabmap/odom"/>
-    
-    <remap from="rgb/image_out"       to="data_odom_sync/image"/>
-    <remap from="depth/image_out"     to="data_odom_sync/depth"/>
-    <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
-    <remap from="odom_out"            to="odom_sync"/>
-  </node>
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
-    <remap from="rgb/image"       to="data_odom_sync/image"/>
-    <remap from="depth/image"     to="data_odom_sync/depth"/>
-    <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
-    <remap from="cloud"           to="voxel_cloud" />
-
-    <param name="voxel_size" type="double" value="0.01"/>
-  </node>
 
 </launch>

launch/tests/test_rgbd_image.launch

@@ -0,0 +1,46 @@
+
+<launch>  
+
+  <arg name="compressed" default="false"/>
+
+  <group ns="camera">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
+      <remap from="rgb/image"       to="rgb/image_rect_color"/>
+      <remap from="depth/image"     to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info" to="rgb/camera_info"/>
+    </node>
+  </group>
+
+   <!-- Nodes -->
+  <group ns="rtabmap">
+  
+      <!-- RGB-D Odometry -->
+      <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+        <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
+        <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
+        <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+        
+	    <param name="Odom/AlignWithGround"  type="string" value="true"/>
+        <param name="frame_id"         type="string" value="camera_link"/>
+      </node>
+    
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+      <param name="subscribe_depth"       type="bool"   value="false"/>
+      <param name="subscribe_rgbd"       type="bool"   value="true"/>
+      <param name="frame_id"             type="string" value="camera_link"/>
+      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
+      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
+    </node>
+  
+    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+      <param name="subscribe_depth"      type="bool"   value="false"/>
+      <param name="subscribe_rgbd"       type="bool"   value="true"/>
+      <param name="frame_id"             type="string" value="camera_link"/>
+      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
+      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
+      <param name="subscribe_odom_info"  type="bool"   value="true"/>
+    </node>
+  
+  </group>
+
+</launch>

msg/NodeData.msg

@@ -24,6 +24,8 @@ float32[] fx
 float32[] fy
 float32[] cx
 float32[] cy
+float32[] width
+float32[] height
 float32 baseline
 # local transform (/base_link -> /camera_link)
 geometry_msgs/Transform[] localTransform

msg/RGBDImage.msg

@@ -0,0 +1,12 @@
+
+Header header
+
+sensor_msgs/CameraInfo cameraInfo
+
+# Raw
+sensor_msgs/Image rgb
+sensor_msgs/Image depth
+
+# Compressed
+sensor_msgs/CompressedImage rgbCompressed
+sensor_msgs/CompressedImage depthCompressed

msg/UserData.msg

@@ -0,0 +1,12 @@
+
+Header header
+
+# OpenCV matrix containing the user data. A matrix of type CV_8UC1 
+# with 1 row is considered to be compressed (with rtabmap::compressData() method).
+# If you have one dimension unsigned 8 bits uncompressed data, make sure to transpose it
+# (to have multiple rows instead of multiple columns) in order to be detected as
+# not compressed.
+uint32 rows
+uint32 cols
+uint32 type
+uint8[] data

nodelet_plugins.xml

@@ -103,6 +103,14 @@
       This is my nodelet.
     </description>
   </class>
+  
+  <class name="rtabmap_ros/rgbd_sync" 
+         type="rtabmap_ros::RGBDSync" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
 
 </library>
 

src/CommonDataSubscriber.cpp

@@ -0,0 +1,365 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_ros/CommonDataSubscriber.h>
+
+namespace rtabmap_ros {
+
+
+CommonDataSubscriber::CommonDataSubscriber() :
+		queueSize_(10),
+		subscribedToDepth_(true),
+		subscribedToStereo_(false),
+		subscribedToRGBD_(false),
+		subscribedToScan2d_(false),
+		subscribedToScan3d_(false),
+		subscribedToOdomInfo_(false),
+
+		// RGB + Depth
+		SYNC_INIT(depth),
+		SYNC_INIT(depthScan2d),
+		SYNC_INIT(depthScan3d),
+		SYNC_INIT(depthInfo),
+
+		// RGB + Depth + Odom
+		SYNC_INIT(depthOdom),
+		SYNC_INIT(depthOdomScan2d),
+		SYNC_INIT(depthOdomScan3d),
+		SYNC_INIT(depthOdomInfo),
+
+		// RGB + Depth + User Data
+		SYNC_INIT(depthData),
+		SYNC_INIT(depthDataScan2d),
+		SYNC_INIT(depthDataScan3d),
+		SYNC_INIT(depthDataInfo),
+
+		// RGB + Depth + Odom + User Data
+		SYNC_INIT(depthOdomData),
+		SYNC_INIT(depthOdomDataScan2d),
+		SYNC_INIT(depthOdomDataScan3d),
+		SYNC_INIT(depthOdomDataInfo),
+
+		// Stereo
+		SYNC_INIT(stereo),
+		SYNC_INIT(stereoInfo),
+
+		// Stereo + Odom
+		SYNC_INIT(stereoOdom),
+		SYNC_INIT(stereoOdomInfo),
+
+		// 1 RGBD
+		SYNC_INIT(rgbdScan2d),
+		SYNC_INIT(rgbdScan3d),
+		SYNC_INIT(rgbdInfo),
+
+		// 1 RGBD + Odom
+		SYNC_INIT(rgbdOdom),
+		SYNC_INIT(rgbdOdomScan2d),
+		SYNC_INIT(rgbdOdomScan3d),
+		SYNC_INIT(rgbdOdomInfo),
+
+		// 1 RGBD + User Data
+		SYNC_INIT(rgbdData),
+		SYNC_INIT(rgbdDataScan2d),
+		SYNC_INIT(rgbdDataScan3d),
+		SYNC_INIT(rgbdDataInfo),
+
+		// 1 RGBD + Odom + User Data
+		SYNC_INIT(rgbdOdomData),
+		SYNC_INIT(rgbdOdomDataScan2d),
+		SYNC_INIT(rgbdOdomDataScan3d),
+		SYNC_INIT(rgbdOdomDataInfo),
+
+		// 2 RGBD
+		SYNC_INIT(rgbd2),
+		SYNC_INIT(rgbd2Scan2d),
+		SYNC_INIT(rgbd2Scan3d),
+		SYNC_INIT(rgbd2Info),
+
+		// 2 RGBD + Odom
+		SYNC_INIT(rgbd2Odom),
+		SYNC_INIT(rgbd2OdomScan2d),
+		SYNC_INIT(rgbd2OdomScan3d),
+		SYNC_INIT(rgbd2OdomInfo),
+
+		// 2 RGBD + User Data
+		SYNC_INIT(rgbd2Data),
+		SYNC_INIT(rgbd2DataScan2d),
+		SYNC_INIT(rgbd2DataScan3d),
+		SYNC_INIT(rgbd2DataInfo),
+
+		// 2 RGBD + Odom + User Data
+		SYNC_INIT(rgbd2OdomData),
+		SYNC_INIT(rgbd2OdomDataScan2d),
+		SYNC_INIT(rgbd2OdomDataScan3d),
+		SYNC_INIT(rgbd2OdomDataInfo)
+
+{
+	ros::NodeHandle pnh("~");
+
+	bool subscribeScan2d = false;
+	bool subscribeScan3d = false;
+	bool subscribeOdomInfo = false;
+	bool subscribeUserData = false;
+	int rgbdCameras = 1;
+	bool approxSync = true;
+
+	// ROS related parameters (private)
+	pnh.param("subscribe_depth",     subscribedToDepth_, subscribedToDepth_);
+	if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
+	{
+		ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
+	}
+	pnh.param("subscribe_scan",      subscribeScan2d, subscribeScan2d);
+	pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
+	pnh.param("subscribe_stereo",    subscribedToStereo_, subscribedToStereo_);
+	pnh.param("subscribe_rgbd",      subscribedToRGBD_, subscribedToRGBD_);
+	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
+	pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
+	if(subscribedToDepth_ && subscribedToStereo_)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
+		subscribedToDepth_ = false;
+	}
+	if(subscribedToDepth_ && subscribedToRGBD_)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
+		subscribedToDepth_ = false;
+	}
+	if(subscribedToStereo_ && subscribedToRGBD_)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
+		subscribedToStereo_ = false;
+	}
+	if(subscribeScan2d && subscribeScan3d)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
+		subscribeScan3d = false;
+	}
+	if(subscribeScan2d || subscribeScan3d)
+	{
+		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_)
+		{
+			ROS_WARN("When subscribing to laser scan, you should subscribe to depth, stereo or rgbd too. Subscribing to depth by default...");
+			subscribedToDepth_ = true;
+		}
+	}
+	if(subscribedToStereo_)
+	{
+		approxSync = false; // default for stereo: exact sync
+	}
+
+	std::string odomFrameId;
+	pnh.getParam("odom_frame_id", odomFrameId);
+	pnh.param("rgbd_cameras",       rgbdCameras, rgbdCameras);
+	if(pnh.hasParam("depth_cameras"))
+	{
+		ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
+	}
+	pnh.param("queue_size",          queueSize_, queueSize_);
+	if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
+	{
+		ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
+				 "to \"approx_sync\"! Your value is still copied to "
+				 "corresponding parameter.");
+		pnh.param("stereo_approx_sync", approxSync, approxSync);
+	}
+	else
+	{
+		pnh.param("approx_sync", approxSync, approxSync);
+	}
+
+	if(rgbdCameras <= 0 && subscribedToRGBD_)
+	{
+		rgbdCameras = 1;
+	}
+
+	ROS_INFO("%s: queue_size    = %d", ros::this_node::getName().c_str(), queueSize_);
+	ROS_INFO("%s: rgbd_cameras = %d", ros::this_node::getName().c_str(), rgbdCameras);
+	ROS_INFO("%s: approx_sync   = %s", ros::this_node::getName().c_str(), approxSync?"true":"false");
+
+	bool subscribeOdom = odomFrameId.empty();
+	if(subscribedToDepth_)
+	{
+		setupDepthCallbacks(
+				subscribeOdom,
+				subscribeUserData,
+				subscribeScan2d,
+				subscribeScan3d,
+				subscribeOdomInfo,
+				queueSize_,
+				approxSync);
+	}
+	else if(subscribedToStereo_)
+	{
+		setupStereoCallbacks(
+				subscribeOdom,
+				subscribeOdomInfo,
+				queueSize_,
+				approxSync);
+	}
+	else if(subscribedToRGBD_)
+	{
+		if(rgbdCameras == 2)
+		{
+			setupRGBD2Callbacks(
+					subscribeOdom,
+					subscribeUserData,
+					subscribeScan2d,
+					subscribeScan3d,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync);
+		}
+		else
+		{
+			setupRGBDCallbacks(
+					subscribeOdom,
+					subscribeUserData,
+					subscribeScan2d,
+					subscribeScan3d,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync);
+		}
+	}
+}
+
+CommonDataSubscriber::~CommonDataSubscriber()
+{
+	// RGB + Depth
+	SYNC_DEL(depth);
+	SYNC_DEL(depthScan2d);
+	SYNC_DEL(depthScan3d);
+	SYNC_DEL(depthInfo);
+
+	// RGB + Depth + Odom
+	SYNC_DEL(depthOdom);
+	SYNC_DEL(depthOdomScan2d);
+	SYNC_DEL(depthOdomScan3d);
+	SYNC_DEL(depthOdomInfo);
+
+	// RGB + Depth + User Data
+	SYNC_DEL(depthData);
+	SYNC_DEL(depthDataScan2d);
+	SYNC_DEL(depthDataScan3d);
+	SYNC_DEL(depthDataInfo);
+
+	// RGB + Depth + Odom + User Data
+	SYNC_DEL(depthOdomData);
+	SYNC_DEL(depthOdomDataScan2d);
+	SYNC_DEL(depthOdomDataScan3d);
+	SYNC_DEL(depthOdomDataInfo);
+
+	// Stereo
+	SYNC_DEL(stereo);
+	SYNC_DEL(stereoInfo);
+
+	// Stereo + Odom
+	SYNC_DEL(stereoOdom);
+	SYNC_DEL(stereoOdomInfo);
+
+	// 1 RGBD
+	SYNC_DEL(rgbdScan2d);
+	SYNC_DEL(rgbdScan3d);
+	SYNC_DEL(rgbdInfo);
+
+	// 1 RGBD + Odom
+	SYNC_DEL(rgbdOdom);
+	SYNC_DEL(rgbdOdomScan2d);
+	SYNC_DEL(rgbdOdomScan3d);
+	SYNC_DEL(rgbdOdomInfo);
+
+	// 1 RGBD + User Data
+	SYNC_DEL(rgbdData);
+	SYNC_DEL(rgbdDataScan2d);
+	SYNC_DEL(rgbdDataScan3d);
+	SYNC_DEL(rgbdDataInfo);
+
+	// 1 RGBD + Odom + User Data
+	SYNC_DEL(rgbdOdomData);
+	SYNC_DEL(rgbdOdomDataScan2d);
+	SYNC_DEL(rgbdOdomDataScan3d);
+	SYNC_DEL(rgbdOdomDataInfo);
+
+	// 2 RGBD
+	SYNC_DEL(rgbd2);
+	SYNC_DEL(rgbd2Scan2d);
+	SYNC_DEL(rgbd2Scan3d);
+	SYNC_DEL(rgbd2Info);
+
+	// 2 RGBD + Odom
+	SYNC_DEL(rgbd2Odom);
+	SYNC_DEL(rgbd2OdomScan2d);
+	SYNC_DEL(rgbd2OdomScan3d);
+	SYNC_DEL(rgbd2OdomInfo);
+
+	// 2 RGBD + User Data
+	SYNC_DEL(rgbd2Data);
+	SYNC_DEL(rgbd2DataScan2d);
+	SYNC_DEL(rgbd2DataScan3d);
+	SYNC_DEL(rgbd2DataInfo);
+
+	// 2 RGBD + Odom + User Data
+	SYNC_DEL(rgbd2OdomData);
+	SYNC_DEL(rgbd2OdomDataScan2d);
+	SYNC_DEL(rgbd2OdomDataScan3d);
+	SYNC_DEL(rgbd2OdomDataInfo);
+
+	for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
+	{
+		delete rgbdSubs_[i];
+	}
+	rgbdSubs_.clear();
+}
+
+void CommonDataSubscriber::commonSingleDepthCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const cv_bridge::CvImageConstPtr & imageMsg,
+		const cv_bridge::CvImageConstPtr & depthMsg,
+		const sensor_msgs::CameraInfo & cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
+	std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
+	std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
+	if(imageMsg.get())
+	{
+		imageMsgs.push_back(imageMsg);
+	}
+	if(depthMsg.get())
+	{
+		depthMsgs.push_back(depthMsg);
+	}
+	cameraInfoMsgs.push_back(cameraInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+} /* namespace rtabmap_ros */

src/CoreNode.cpp

@@ -89,7 +89,7 @@ int main(int argc, char** argv)
 
 	rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
 
-	CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart, parameters);
+	rtabmap_ros::CoreWrapper * rtabmap = new rtabmap_ros::CoreWrapper(deleteDbOnStart, parameters);
 
 	ROS_INFO("rtabmap %s started...", RTABMAP_VERSION);
 	ros::spin();

src/GuiNode.cpp

@@ -54,7 +54,7 @@ int main(int argc, char** argv)
 	app = new QApplication(argc, argv);
 	app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) );
 
-	GuiWrapper * gui = new GuiWrapper(argc, argv);
+	rtabmap_ros::GuiWrapper * gui = new rtabmap_ros::GuiWrapper(argc, argv);
 
 	// Catch ctrl-c to close the gui
 	// (Place this after QApplication's constructor)

src/MsgConversion.cpp

@@ -40,7 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_geometry/pinhole_camera_model.h>
 #include <image_geometry/stereo_camera_model.h>
 #include <sensor_msgs/image_encodings.h>
-#include <cv_bridge/cv_bridge.h>
 #include <laser_geometry/laser_geometry.h>
 #include <rtabmap/core/util3d_surface.h>
 
@@ -119,6 +118,78 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg)
 	return rtabmap::Transform::fromEigen3d(tfPose);
 }
 
+void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth)
+{
+	if(!image.rgb.data.empty())
+	{
+		rgb = cv_bridge::toCvCopy(image.rgb);
+	}
+	else if(!image.rgbCompressed.data.empty())
+	{
+		rgb = cv_bridge::toCvCopy(image.rgbCompressed);
+	}
+	else
+	{
+		// empty
+		rgb = boost::make_shared<cv_bridge::CvImage>();
+	}
+
+	if(!image.depth.data.empty())
+	{
+		depth = cv_bridge::toCvCopy(image.depth);
+	}
+	else if(!image.depthCompressed.data.empty())
+	{
+		cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
+		ptr->header = image.depthCompressed.header;
+		ptr->image = rtabmap::uncompressImage(image.depthCompressed.data);
+		ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
+		ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
+		depth = ptr;
+	}
+	else
+	{
+		// empty
+		depth = boost::make_shared<cv_bridge::CvImage>();
+	}
+}
+
+void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth)
+{
+	if(!image->rgb.data.empty())
+	{
+		rgb = cv_bridge::toCvShare(image->rgb, image);
+	}
+	else if(!image->rgbCompressed.data.empty())
+	{
+		rgb = cv_bridge::toCvCopy(image->rgbCompressed);
+	}
+	else
+	{
+		// empty
+		rgb = boost::make_shared<cv_bridge::CvImage>();
+	}
+
+	if(!image->depth.data.empty())
+	{
+		depth = cv_bridge::toCvShare(image->depth, image);
+	}
+	else if(!image->depthCompressed.data.empty())
+	{
+		cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
+		ptr->header = image->depthCompressed.header;
+		ptr->image = rtabmap::uncompressImage(image->depthCompressed.data);
+		ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
+		ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
+		depth = ptr;
+	}
+	else
+	{
+		// empty
+		depth = boost::make_shared<cv_bridge::CvImage>();
+	}
+}
+
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes)
 {
 	UASSERT(compressed.empty() || compressed.type() == CV_8UC1);
@@ -566,9 +637,11 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 	{
 		// stereo model
 		if(msg.fx.size() == 1 &&
-		   msg.fy.size() == 1,
-		   msg.cx.size() == 1,
-		   msg.cy.size() == 1,
+		   msg.fy.size() == 1 &&
+		   msg.cx.size() == 1 &&
+		   msg.cy.size() == 1 &&
+		   msg.width.size() == 1 &&
+		   msg.height.size() == 1 &&
 		   msg.localTransform.size() == 1)
 		{
 			stereoModel = rtabmap::StereoCameraModel(
@@ -577,7 +650,8 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 					msg.cx[0],
 					msg.cy[0],
 					msg.baseline,
-					transformFromGeometryMsg(msg.localTransform[0]));
+					transformFromGeometryMsg(msg.localTransform[0]),
+					cv::Size(msg.width[0], msg.height[0]));
 		}
 	}
 	else
@@ -596,7 +670,9 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 						msg.fy[i],
 						msg.cx[i],
 						msg.cy[i],
-						transformFromGeometryMsg(msg.localTransform[i])));
+						transformFromGeometryMsg(msg.localTransform[i]),
+						0.0,
+						cv::Size(msg.width[i], msg.height[i])));
 			}
 		}
 	}
@@ -672,6 +748,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 		msg.fy.resize(signature.sensorData().cameraModels().size());
 		msg.cx.resize(signature.sensorData().cameraModels().size());
 		msg.cy.resize(signature.sensorData().cameraModels().size());
+		msg.width.resize(signature.sensorData().cameraModels().size());
+		msg.height.resize(signature.sensorData().cameraModels().size());
 		msg.localTransform.resize(signature.sensorData().cameraModels().size());
 		for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
 		{
@@ -679,6 +757,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 			msg.fy[i] = signature.sensorData().cameraModels()[i].fy();
 			msg.cx[i] = signature.sensorData().cameraModels()[i].cx();
 			msg.cy[i] = signature.sensorData().cameraModels()[i].cy();
+			msg.width[i] = signature.sensorData().cameraModels()[i].imageWidth();
+			msg.height[i] = signature.sensorData().cameraModels()[i].imageHeight();
 			transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
 		}
 	}
@@ -688,6 +768,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 		msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
 		msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx());
 		msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy());
+		msg.width.push_back(signature.sensorData().stereoCameraModel().left().imageWidth());
+		msg.height.push_back(signature.sensorData().stereoCameraModel().left().imageHeight());
 		msg.baseline = signature.sensorData().stereoCameraModel().baseline();
 		msg.localTransform.resize(1);
 		transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.localTransform[0]);
@@ -868,6 +950,52 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.localScanMap = rtabmap::compressData(info.localScanMap);
 }
 
+cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg)
+{
+	cv::Mat data;
+	if(!dataMsg.data.empty())
+	{
+		if(dataMsg.cols > 0 && dataMsg.rows > 0 && dataMsg.type >= 0)
+		{
+			data = cv::Mat(dataMsg.rows, dataMsg.cols, dataMsg.type, (void*)dataMsg.data.data()).clone();
+		}
+		else
+		{
+			if(dataMsg.cols != (int)dataMsg.data.size() || dataMsg.rows != 1 || dataMsg.type != CV_8UC1)
+			{
+				ROS_ERROR("cols, rows and type fields of the UserData msg "
+						"are not correctly set (cols=%d, rows=%d, type=%d)! We assume that the data "
+						"is compressed (cols=%d, rows=1, type=%d(CV_8UC1)).",
+						dataMsg.cols, dataMsg.rows, dataMsg.type, (int)dataMsg.data.size(), CV_8UC1);
+
+			}
+			data = cv::Mat(1, dataMsg.data.size(), CV_8UC1, (void*)dataMsg.data.data()).clone();
+		}
+	}
+	return data;
+}
+void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress)
+{
+	if(!data.empty())
+	{
+		if(compress)
+		{
+			dataMsg.data = rtabmap::compressData(data);
+			dataMsg.rows = 1;
+			dataMsg.cols = dataMsg.data.size();
+			dataMsg.type = CV_8UC1;
+		}
+		else
+		{
+			dataMsg.data.resize(data.step[0] * data.rows); // use step for non-contiguous matrices
+			memcpy(dataMsg.data.data(), data.data, dataMsg.data.size());
+			dataMsg.rows = data.rows;
+			dataMsg.cols = data.cols;
+			dataMsg.type = data.type();
+		}
+	}
+}
+
 rtabmap::Transform getTransform(
 		const std::string & fromFrameId,
 		const std::string & toFrameId,
@@ -940,9 +1068,9 @@ rtabmap::Transform getTransform(
 }
 
 bool convertRGBDMsgs(
-		const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
-		const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
-		const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
+		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+		const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
@@ -956,8 +1084,8 @@ bool convertRGBDMsgs(
 			imageMsgs.size() == depthMsgs.size() &&
 			imageMsgs.size() == cameraInfoMsgs.size());
 
-	int imageWidth = imageMsgs[0]->width;
-	int imageHeight = imageMsgs[0]->height;
+	int imageWidth = imageMsgs[0]->image.cols;
+	int imageHeight = imageMsgs[0]->image.rows;
 	int cameraCount = imageMsgs.size();
 	for(unsigned int i=0; i<imageMsgs.size(); ++i)
 	{
@@ -974,18 +1102,18 @@ bool convertRGBDMsgs(
 			return false;
 		}
 
-		UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
+		UASSERT_MSG(imageMsgs[i]->image.cols == imageWidth && imageMsgs[i]->image.rows == imageHeight,
 				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
 						imageWidth,
-						imageMsgs[i]->width,
+						imageMsgs[i]->image.cols,
 						imageHeight,
-						imageMsgs[i]->height).c_str());
-		UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
+						imageMsgs[i]->image.rows).c_str());
+		UASSERT_MSG(depthMsgs[i]->image.cols == imageWidth && depthMsgs[i]->image.rows == imageHeight,
 				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
 						imageWidth,
-						depthMsgs[i]->width,
+						depthMsgs[i]->image.cols,
 						imageHeight,
-						depthMsgs[i]->height).c_str());
+						depthMsgs[i]->image.rows).c_str());
 
 		rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform);
 		if(localTransform.isNull())
@@ -1015,21 +1143,22 @@ bool convertRGBDMsgs(
 			}
 		}
 
-		cv_bridge::CvImageConstPtr ptrImage;
-		if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+		cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i];
+		if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+		   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
 		{
-			ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
+			// do nothing
 		}
-		else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-		   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+		else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
 		{
-			ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
+			ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8");
 		}
 		else
 		{
-			ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
+			ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8");
 		}
-		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
+		cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
 		cv::Mat subDepth = ptrDepth->image;
 
 		// initialize
@@ -1062,7 +1191,7 @@ bool convertRGBDMsgs(
 			return false;
 		}
 
-		cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
+		cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
 	}
 	return true;
 }

src/impl/CommonDataSubscriberDepth.cpp

@@ -0,0 +1,369 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_ros/CommonDataSubscriber.h>
+
+namespace rtabmap_ros {
+
+// RGB + Depth
+void CommonDataSubscriber::depthCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan2dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan3dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// RGB + Depth + Odom
+void CommonDataSubscriber::depthOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// RGB + Depth + User Data
+void CommonDataSubscriber::depthDataCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// RGB + Depth + Odom + User Data
+void CommonDataSubscriber::depthOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupDepthCallbacks(
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup depth callback");
+	ros::NodeHandle nh; // public
+	ros::NodeHandle pnh("~"); // private
+
+	std::string rgbPrefix = "rgb";
+	std::string depthPrefix = "depth";
+	ros::NodeHandle rgb_nh(nh, rgbPrefix);
+	ros::NodeHandle depth_nh(nh, depthPrefix);
+	ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
+	ros::NodeHandle depth_pnh(pnh, depthPrefix);
+	image_transport::ImageTransport rgb_it(rgb_nh);
+	image_transport::ImageTransport depth_it(depth_nh);
+	image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+	image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+	imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+	imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+	cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+		userDataSub_.subscribe(nh, "user_data", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+	else if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+	else
+	{
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGBD.cpp

@@ -0,0 +1,386 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_ros/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+// 1 RGBD camera
+void CommonDataSubscriber::rgbdCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdScan2dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdScan3dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdInfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 1 RGBD camera + Odom
+void CommonDataSubscriber::rgbdOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 1 RGBD camera + User Data
+void CommonDataSubscriber::rgbdDataCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdDataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdDataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdDataInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 1 RGBD camera + Odom + User Data
+void CommonDataSubscriber::rgbdOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupRGBDCallbacks(
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd callback");
+	ros::NodeHandle nh; // public
+	ros::NodeHandle pnh("~"); // private
+
+	if(subscribeOdom || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo)
+	{
+		rgbdSubs_.resize(1);
+		rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
+		rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1);
+
+
+		if(subscribeOdom && subscribeUserData)
+		{
+			odomSub_.subscribe(nh, "odom", 1);
+			userDataSub_.subscribe(nh, "user_data", 1);
+			if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", 1);
+				SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
+			}
+		}
+		else if(subscribeOdom)
+		{
+			odomSub_.subscribe(nh, "odom", 1);
+			if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", 1);
+				SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
+			}
+		}
+		else if(subscribeUserData)
+		{
+			userDataSub_.subscribe(nh, "user_data", 1);
+			if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", 1);
+				SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
+			}
+		}
+		else
+		{
+			if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", 1);
+				SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				ROS_FATAL("Not supposed to be here!");
+			}
+		}
+	}
+	else
+	{
+		rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);
+
+		ROS_INFO("\n%s subscribed to:\n   %s",
+				ros::this_node::getName().c_str(),
+				rgbdSub_.getTopic().c_str());
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGBD2.cpp

@@ -0,0 +1,385 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_ros/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+#define IMAGE_CONVERSION() \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
+		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->cameraInfo); \
+		cameraInfoMsgs.push_back(image2Msg->cameraInfo);
+
+// 2 RGBD
+void CommonDataSubscriber::rgbd2Callback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2Scan2dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2Scan3dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2InfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 2 RGBD + Odom
+void CommonDataSubscriber::rgbd2OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 2 RGBD + User Data
+void CommonDataSubscriber::rgbd2DataCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2DataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2DataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2DataInfoCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 2 RGBD + Odom + User Data
+void CommonDataSubscriber::rgbd2OdomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupRGBD2Callbacks(
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd2 callback");
+	ros::NodeHandle nh; // public
+	ros::NodeHandle pnh("~"); // private
+
+	rgbdSubs_.resize(2);
+	for(int i=0; i<2; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
+	}
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+		userDataSub_.subscribe(nh, "user_data", 1);
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+	else if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", 1);
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+	else
+	{
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberStereo.cpp

@@ -0,0 +1,138 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_ros/CommonDataSubscriber.h>
+
+namespace rtabmap_ros {
+
+// Stereo
+void CommonDataSubscriber::stereoCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::stereoInfoCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+	   const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// Stereo + Odom
+void CommonDataSubscriber::stereoOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::stereoOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupStereoCallbacks(
+		bool subscribeOdom,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup stereo callback");
+	ros::NodeHandle nh; // public
+	ros::NodeHandle pnh("~"); // private
+
+	ros::NodeHandle left_nh(nh, "left");
+	ros::NodeHandle right_nh(nh, "right");
+	ros::NodeHandle left_pnh(pnh, "left");
+	ros::NodeHandle right_pnh(pnh, "right");
+	image_transport::ImageTransport left_it(left_nh);
+	image_transport::ImageTransport right_it(right_nh);
+	image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
+	image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
+
+	imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
+	imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
+	cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
+	cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
+
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL6(stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		}
+	}
+	else
+	{
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/nodelets/rgbd_odometry.cpp

@@ -63,7 +63,8 @@ public:
 		OdometryROS(false, true, false),
 		approxSync_(0),
 		exactSync_(0),
-		sync2_(0),
+		approxSync2_(0),
+		exactSync2_(0),
 		queueSize_(5)
 	{
 	}
@@ -78,9 +79,13 @@ public:
 		{
 			delete exactSync_;
 		}
-		if(sync2_)
+		if(approxSync2_)
 		{
-			delete sync2_;
+			delete approxSync2_;
+		}
+		if(exactSync2_)
+		{
+			delete exactSync2_;
 		}
 	}
 
@@ -91,65 +96,50 @@ private:
 		ros::NodeHandle & nh = getNodeHandle();
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
-		int depthCameras = 1;
+		int rgbdCameras = 1;
 		bool approxSync = true;
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
-		pnh.param("depth_cameras", depthCameras, depthCameras);
-		if(depthCameras <= 0)
+		if(pnh.hasParam("depth_cameras"))
 		{
-			depthCameras = 1;
+			ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics.");
 		}
-		if(depthCameras > 2)
+		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
+		if(rgbdCameras <= 0)
+		{
+			rgbdCameras = 1;
+		}
+		if(rgbdCameras > 2)
 		{
 			NODELET_FATAL("Only 2 cameras maximum supported yet.");
 		}
 
-		if(depthCameras == 2)
+		if(rgbdCameras == 2)
 		{
-			ros::NodeHandle rgb0_nh(nh, "rgb0");
-			ros::NodeHandle depth0_nh(nh, "depth0");
-			ros::NodeHandle rgb0_pnh(pnh, "rgb0");
-			ros::NodeHandle depth0_pnh(pnh, "depth0");
-			image_transport::ImageTransport rgb0_it(rgb0_nh);
-			image_transport::ImageTransport depth0_it(depth0_nh);
-			image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh);
-			image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh);
+			rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
+			rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
 
-			image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
-			image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
-			info_sub_.subscribe(rgb0_nh, "camera_info", 1);
-
-			ros::NodeHandle rgb1_nh(nh, "rgb1");
-			ros::NodeHandle depth1_nh(nh, "depth1");
-			ros::NodeHandle rgb1_pnh(pnh, "rgb1");
-			ros::NodeHandle depth1_pnh(pnh, "depth1");
-			image_transport::ImageTransport rgb1_it(rgb1_nh);
-			image_transport::ImageTransport depth1_it(depth1_nh);
-			image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh);
-			image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh);
-
-			image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
-			image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1);
-			info2_sub_.subscribe(rgb1_nh, "camera_info", 1);
-
-			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
-					MySync2Policy(queueSize_),
-					image_mono_sub_,
-					image_depth_sub_,
-					info_sub_,
-					image_mono2_sub_,
-					image_depth2_sub_,
-					info2_sub_);
-			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
-			NODELET_INFO("\n%s subscribed to (approx sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+			if(approxSync)
+			{
+				approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+						MyApproxSync2Policy(queueSize_),
+						rgbd_image1_sub_,
+						rgbd_image2_sub_);
+				approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
+			}
+			else
+			{
+				exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+						MyExactSync2Policy(queueSize_),
+						rgbd_image1_sub_,
+						rgbd_image2_sub_);
+				exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
+			}
+			NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s",
 					ros::this_node::getName().c_str(),
-					image_mono_sub_.getTopic().c_str(),
-					image_depth_sub_.getTopic().c_str(),
-					info_sub_.getTopic().c_str(),
-					image_mono2_sub_.getTopic().c_str(),
-					image_depth2_sub_.getTopic().c_str(),
-					info2_sub_.getTopic().c_str());
+					approxSync?"approx":"exact",
+					rgbd_image1_sub_.getTopic().c_str(),
+					rgbd_image2_sub_.getTopic().c_str());
 		}
 		else
 		{
@@ -197,6 +187,117 @@ private:
 		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
 	}
 
+	void commonCallback(
+				const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
+				const std::vector<sensor_msgs::CameraInfo>& cameraInfos)
+	{
+		ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
+		ros::Time higherStamp;
+		int imageWidth = rgbImages[0]->image.cols;
+		int imageHeight = rgbImages[0]->image.rows;
+		int cameraCount = rgbImages.size();
+		cv::Mat rgb;
+		cv::Mat depth;
+		pcl::PointCloud<pcl::PointXYZ> scanCloud;
+		std::vector<CameraModel> cameraModels;
+		for(unsigned int i=0; i<rgbImages.size(); ++i)
+		{
+			if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+				!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+			{
+				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+				return;
+			}
+			UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
+					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+							imageWidth,
+							rgbImages[i]->image.cols,
+							imageHeight,
+							rgbImages[i]->image.rows).c_str());
+			UASSERT_MSG(depthImages[i]->image.cols == imageWidth && depthImages[i]->image.rows == imageHeight,
+					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+							imageWidth,
+							depthImages[i]->image.cols,
+							imageHeight,
+							depthImages[i]->image.rows).c_str());
+
+			ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+
+			if(i == 0)
+			{
+				higherStamp = stamp;
+			}
+			else if(stamp > higherStamp)
+			{
+				higherStamp = stamp;
+			}
+
+			Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp);
+			if(localTransform.isNull())
+			{
+				return;
+			}
+
+			cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
+			if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+			   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+			{
+				ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+			}
+
+			cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+			cv::Mat subDepth = ptrDepth->image;
+
+			// initialize
+			if(rgb.empty())
+			{
+				rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+			}
+			if(depth.empty())
+			{
+				depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
+			}
+
+			if(ptrImage->image.type() == rgb.type())
+			{
+				ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+			}
+			else
+			{
+				NODELET_ERROR("Some RGB images are not the same type!");
+				return;
+			}
+
+			if(subDepth.type() == depth.type())
+			{
+				subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+			}
+			else
+			{
+				NODELET_ERROR("Some Depth images are not the same type!");
+				return;
+			}
+
+			cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform));
+		}
+
+		rtabmap::SensorData data(
+				rgb,
+				depth,
+				cameraModels,
+				0,
+				rtabmap_ros::timestampFromROS(higherStamp));
+
+		this->processData(data, higherStamp);
+	}
+
 	void callback(
 			const sensor_msgs::ImageConstPtr& image,
 			const sensor_msgs::ImageConstPtr& depth,
@@ -204,178 +305,32 @@ private:
 	{
 		if(!this->isPaused())
 		{
-			if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
-			   !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
-				 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
-				 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
-			{
-				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
-						  "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
-						image->encoding.c_str(), depth->encoding.c_str());
-				return;
-			}
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			imageMsgs[0] = cv_bridge::toCvShare(image);
+			depthMsgs[0] = cv_bridge::toCvShare(depth);
+			infoMsgs.push_back(*cameraInfo);
 
-			// use the highest stamp to make sure that there will be no future interpolation required when synchronized with another node
-			ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp;
-
-			Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
-			if(localTransform.isNull())
-			{
-				return;
-			}
-
-			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
-			{
-				rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
-				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
-				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
-
-				rtabmap::SensorData data(
-						ptrImage->image,
-						ptrDepth->image,
-						rtabmapModel,
-						0,
-						rtabmap_ros::timestampFromROS(stamp));
-
-				this->processData(data, stamp);
-			}
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
 	}
 
 	void callback2(
-			const sensor_msgs::ImageConstPtr& image,
-			const sensor_msgs::ImageConstPtr& depth,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
-			const sensor_msgs::ImageConstPtr& image2,
-			const sensor_msgs::ImageConstPtr& depth2,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo2)
+			const rtabmap_ros::RGBDImageConstPtr& image,
+			const rtabmap_ros::RGBDImageConstPtr& image2)
 	{
 		if(!this->isPaused())
 		{
-			std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
-			std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
-			std::vector<sensor_msgs::CameraInfoConstPtr> infoMsgs;
-			imageMsgs.push_back(image);
-			imageMsgs.push_back(image2);
-			depthMsgs.push_back(depth);
-			depthMsgs.push_back(depth2);
-			infoMsgs.push_back(cameraInfo);
-			infoMsgs.push_back(cameraInfo2);
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+			rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+			infoMsgs.push_back(image->cameraInfo);
+			infoMsgs.push_back(image2->cameraInfo);
 
-			ros::Time higherStamp;
-			int imageWidth = imageMsgs[0]->width;
-			int imageHeight = imageMsgs[0]->height;
-			int cameraCount = imageMsgs.size();
-			cv::Mat rgb;
-			cv::Mat depth;
-			pcl::PointCloud<pcl::PointXYZ> scanCloud;
-			std::vector<CameraModel> cameraModels;
-			for(unsigned int i=0; i<imageMsgs.size(); ++i)
-			{
-				if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
-					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
-					!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
-					 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
-					 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
-				{
-					NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
-					return;
-				}
-				UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
-						uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
-								imageWidth,
-								imageMsgs[i]->width,
-								imageHeight,
-								imageMsgs[i]->height).c_str());
-				UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
-						uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
-								imageWidth,
-								depthMsgs[i]->width,
-								imageHeight,
-								depthMsgs[i]->height).c_str());
-
-				ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
-
-				if(i == 0)
-				{
-					higherStamp = stamp;
-				}
-				else if(stamp > higherStamp)
-				{
-					higherStamp = stamp;
-				}
-
-				Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp);
-				if(localTransform.isNull())
-				{
-					return;
-				}
-
-				cv_bridge::CvImageConstPtr ptrImage;
-				if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
-				{
-					ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
-				}
-				else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-				   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-				{
-					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
-				}
-				else
-				{
-					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
-				}
-				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
-				cv::Mat subDepth = ptrDepth->image;
-
-				// initialize
-				if(rgb.empty())
-				{
-					rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
-				}
-				if(depth.empty())
-				{
-					depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
-				}
-
-				if(ptrImage->image.type() == rgb.type())
-				{
-					ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
-				}
-				else
-				{
-					NODELET_ERROR("Some RGB images are not the same type!");
-					return;
-				}
-
-				if(subDepth.type() == depth.type())
-				{
-					subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
-				}
-				else
-				{
-					NODELET_ERROR("Some Depth images are not the same type!");
-					return;
-				}
-
-				cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
-			}
-
-			rtabmap::SensorData data(
-					rgb,
-					depth,
-					cameraModels,
-					0,
-					rtabmap_ros::timestampFromROS(higherStamp));
-
-			this->processData(data, higherStamp);
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
 	}
 
@@ -395,18 +350,23 @@ protected:
 			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
 			exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 		}
-		if(sync2_)
+		if(approxSync2_)
 		{
-			delete sync2_;
-			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
-					MySync2Policy(queueSize_),
-					image_mono_sub_,
-					image_depth_sub_,
-					info_sub_,
-					image_mono2_sub_,
-					image_depth2_sub_,
-					info2_sub_);
-			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
+			delete approxSync2_;
+			approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+					MyApproxSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
+		}
+		if(exactSync2_)
+		{
+			delete exactSync2_;
+			exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+					MyExactSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
 		}
 	}
 
@@ -414,15 +374,18 @@ private:
 	image_transport::SubscriberFilter image_mono_sub_;
 	image_transport::SubscriberFilter image_depth_sub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
-	image_transport::SubscriberFilter image_mono2_sub_;
-	image_transport::SubscriberFilter image_depth2_sub_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
+
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
+
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
 	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
-	message_filters::Synchronizer<MySync2Policy> * sync2_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync2Policy;
+	message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
+	message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
 	int queueSize_;
 };
 

src/nodelets/rgbd_sync.cpp

@@ -0,0 +1,165 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap_ros/RGBDImage.h"
+
+#include "rtabmap/core/Compression.h"
+
+namespace rtabmap_ros
+{
+
+class RGBDSync : public nodelet::Nodelet
+{
+public:
+	RGBDSync() :
+		approxSyncDepth_(0),
+		exactSyncDepth_(0)
+	{}
+
+	virtual ~RGBDSync()
+	{
+		if(approxSyncDepth_)
+			delete approxSyncDepth_;
+		if(exactSyncDepth_)
+			delete exactSyncDepth_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("queue_size", queueSize, queueSize);
+
+		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
+
+		rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
+		rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
+
+		if(approxSync)
+		{
+			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
+		}
+		else
+		{
+			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
+		}
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+	}
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& image,
+			  const sensor_msgs::ImageConstPtr& depth,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
+		{
+			rtabmap_ros::RGBDImage msg;
+			msg.header.frame_id = cameraInfo->header.frame_id;
+			msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
+			msg.cameraInfo = *cameraInfo;
+
+			if(rgbdImageCompressedPub_.getNumSubscribers())
+			{
+				rtabmap_ros::RGBDImage msgCompressed = msg;
+
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+				imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
+
+				cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
+				ROS_ASSERT(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1);
+				msgCompressed.depthCompressed.header = imageDepthPtr->header;
+				msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+				msgCompressed.depthCompressed.format = "png";
+
+				rgbdImageCompressedPub_.publish(msgCompressed);
+			}
+
+			if(rgbdImagePub_.getNumSubscribers())
+			{
+				msg.rgb = *image;
+				msg.depth = *depth;
+				rgbdImagePub_.publish(msg);
+			}
+		}
+	}
+
+private:
+	ros::Publisher rgbdImagePub_;
+	ros::Publisher rgbdImageCompressedPub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDSync, nodelet::Nodelet);
+}
+