Added rgb_sync nodelet. Added sync rgbd5 and rgbd6 callbacks. Removed OdomInfo from sync callbacks having both camera and lidar. Added BAYER_RGGB8 support. point_cloud_assembler: added output frame_id option.

CMakeLists.txt

@@ -209,6 +209,8 @@ IF(RTABMAP_SYNC_MULTI_RGBD)
     src/impl/CommonDataSubscriberRGBD2.cpp
     src/impl/CommonDataSubscriberRGBD3.cpp
     src/impl/CommonDataSubscriberRGBD4.cpp
+    src/impl/CommonDataSubscriberRGBD5.cpp
+    src/impl/CommonDataSubscriberRGBD6.cpp
   )
 ENDIF(RTABMAP_SYNC_MULTI_RGBD)
   
@@ -240,7 +242,7 @@ SET(rtabmap_plugins_lib_src
 )
 
 IF(${cv_bridge_VERSION_MAJOR} GREATER 1 OR ${cv_bridge_VERSION_MINOR} GREATER 10)
-   SET(rtabmap_plugins_lib_src ${rtabmap_plugins_lib_src} src/nodelets/rgbd_sync.cpp src/nodelets/stereo_sync.cpp src/nodelets/rgbd_relay.cpp)
+   SET(rtabmap_plugins_lib_src ${rtabmap_plugins_lib_src} src/nodelets/rgbd_sync.cpp src/nodelets/stereo_sync.cpp src/nodelets/rgb_sync.cpp src/nodelets/rgbd_relay.cpp)
 ELSE()
    ADD_DEFINITIONS("-DCV_BRIDGE_HYDRO")
 ENDIF()   
@@ -319,6 +321,14 @@ add_executable(rtabmap_rgbd_sync src/RGBDSyncNode.cpp)
 target_link_libraries(rtabmap_rgbd_sync ${Libraries})
 set_target_properties(rtabmap_rgbd_sync PROPERTIES OUTPUT_NAME "rgbd_sync")
 
+add_executable(rtabmap_stereo_sync src/StereoSyncNode.cpp)
+target_link_libraries(rtabmap_stereo_sync ${Libraries})
+set_target_properties(rtabmap_stereo_sync PROPERTIES OUTPUT_NAME "stereo_sync")
+
+add_executable(rtabmap_rgb_sync src/RGBSyncNode.cpp)
+target_link_libraries(rtabmap_rgb_sync ${Libraries})
+set_target_properties(rtabmap_rgb_sync PROPERTIES OUTPUT_NAME "rgb_sync")
+
 add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp)
 target_link_libraries(rtabmap_rgbd_relay ${Libraries})
 set_target_properties(rtabmap_rgbd_relay PROPERTIES OUTPUT_NAME "rgbd_relay")

include/rtabmap_ros/CommonDataSubscriber.h

@@ -210,6 +210,28 @@ private:
 			bool subscribeOdomInfo,
 			int queueSize,
 			bool approxSync); 
+	void setupRGBD5Callbacks(
+			ros::NodeHandle & nh,
+			ros::NodeHandle & pnh,
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeScanDesc,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+	void setupRGBD6Callbacks(
+			ros::NodeHandle & nh,
+			ros::NodeHandle & pnh,
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeScanDesc,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
 #endif
 	void setupScanCallbacks(
 			ros::NodeHandle & nh,
@@ -373,9 +395,6 @@ private:
 	DATA_SYNCS2(rgbdScan3d, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2)
 	DATA_SYNCS2(rgbdScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS2(rgbdInfo, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS3(rgbdScan2dInfo, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS3(rgbdScan3dInfo, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS3(rgbdScanDescInfo, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 1 RGBD + Odom
 	DATA_SYNCS2(rgbdOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImage);
@@ -383,9 +402,6 @@ private:
 	DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS3(rgbdOdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(rgbdOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbdOdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbdOdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbdOdomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 1 RGBD + User Data
@@ -394,9 +410,6 @@ private:
 	DATA_SYNCS3(rgbdDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS3(rgbdDataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(rgbdDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbdDataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbdDataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbdDataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 1 RGBD + Odom + User Data
 	DATA_SYNCS3(rgbdOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
@@ -404,9 +417,6 @@ private:
 	DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS4(rgbdOdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbdOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbdOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbdOdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 #ifdef RTABMAP_SYNC_MULTI_RGBD
@@ -416,9 +426,6 @@ private:
 	DATA_SYNCS3(rgbd2Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS3(rgbd2ScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(rgbd2Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbd2Scan2dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbd2Scan3dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS4(rgbd2ScanDescInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 2 RGBD + Odom
 	DATA_SYNCS3(rgbd2Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
@@ -426,9 +433,6 @@ private:
 	DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS4(rgbd2OdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd2OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd2OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd2OdomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 2 RGBD + User Data
@@ -437,9 +441,6 @@ private:
 	DATA_SYNCS4(rgbd2DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS4(rgbd2DataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbd2DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd2DataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd2DataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd2DataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 2 RGBD + Odom + User Data
 	DATA_SYNCS4(rgbd2OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
@@ -447,9 +448,6 @@ private:
 	DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS5(rgbd2OdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd2OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd2OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd2OdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 	// 3 RGBD
@@ -458,9 +456,6 @@ private:
 	DATA_SYNCS4(rgbd3Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS4(rgbd3ScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbd3Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd3Scan2dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd3Scan3dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS5(rgbd3ScanDescInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 3 RGBD + Odom
 	DATA_SYNCS4(rgbd3Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
@@ -468,9 +463,6 @@ private:
 	DATA_SYNCS5(rgbd3OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS5(rgbd3OdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbd3OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd3OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd3OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd3OdomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 3 RGBD + User Data
@@ -479,9 +471,6 @@ private:
 	DATA_SYNCS5(rgbd3DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS5(rgbd3DataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbd3DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd3DataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd3DataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd3DataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 3 RGBD + Odom + User Data
 	DATA_SYNCS5(rgbd3OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
@@ -489,9 +478,6 @@ private:
 	DATA_SYNCS6(rgbd3OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS6(rgbd3OdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS6(rgbd3OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd3OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd3OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd3OdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 	// 4 RGBD
@@ -500,9 +486,6 @@ private:
 	DATA_SYNCS5(rgbd4Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS5(rgbd4ScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbd4Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd4Scan2dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd4Scan3dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS6(rgbd4ScanDescInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 4 RGBD + Odom
 	DATA_SYNCS5(rgbd4Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
@@ -510,9 +493,6 @@ private:
 	DATA_SYNCS6(rgbd4OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS6(rgbd4OdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS6(rgbd4OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd4OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd4OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd4OdomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 4 RGBD + User Data
@@ -521,9 +501,6 @@ private:
 	DATA_SYNCS6(rgbd4DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS6(rgbd4DataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS6(rgbd4DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd4DataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd4DataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS7(rgbd4DataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 4 RGBD + Odom + User Data
 	DATA_SYNCS6(rgbd4OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
@@ -531,10 +508,36 @@ private:
 	DATA_SYNCS7(rgbd4OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
 	DATA_SYNCS7(rgbd4OdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS7(rgbd4OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
-	DATA_SYNCS8(rgbd4OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
-	DATA_SYNCS8(rgbd4OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
-	DATA_SYNCS8(rgbd4OdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
+
+	// 5 RGBD
+	DATA_SYNCS5(rgbd5, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS6(rgbd5Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS6(rgbd5Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS6(rgbd5ScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
+	DATA_SYNCS6(rgbd5Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 5 RGBD + Odom
+	DATA_SYNCS6(rgbd5Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS7(rgbd5OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS7(rgbd5OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS7(rgbd5OdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
+	DATA_SYNCS7(rgbd5OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 6 RGBD
+	DATA_SYNCS6(rgbd6, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS7(rgbd6Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS7(rgbd6Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS7(rgbd6ScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
+	DATA_SYNCS7(rgbd6Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 6 RGBD + Odom
+	DATA_SYNCS7(rgbd6Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS8(rgbd6OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS8(rgbd6OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS8(rgbd6OdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
+	DATA_SYNCS8(rgbd6OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
 #endif //RTABMAP_SYNC_MULTI_RGBD
 
 	// Scan

include/rtabmap_ros/CommonDataSubscriberDefines.h

@@ -247,7 +247,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
 		} \
-		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s", \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s", \
 				name_.c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \

nodelet_plugins.xml

@@ -144,6 +144,14 @@
     </description>
   </class>
   
+  <class name="rtabmap_ros/rgb_sync" 
+         type="rtabmap_ros::RgbSync" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
   <class name="rtabmap_ros/undistort_depth" 
          type="rtabmap_ros::UndistortDepth" 
          base_class_type="nodelet::Nodelet">

src/CommonDataSubscriber.cpp

@@ -140,9 +140,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbdScan3d),
 		SYNC_INIT(rgbdScanDesc),
 		SYNC_INIT(rgbdInfo),
-		SYNC_INIT(rgbdScan2dInfo),
-		SYNC_INIT(rgbdScan3dInfo),
-		SYNC_INIT(rgbdScanDescInfo),
 
 		// 1 RGBD + Odom
 		SYNC_INIT(rgbdOdom),
@@ -150,9 +147,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbdOdomScan3d),
 		SYNC_INIT(rgbdOdomScanDesc),
 		SYNC_INIT(rgbdOdomInfo),
-		SYNC_INIT(rgbdOdomScan2dInfo),
-		SYNC_INIT(rgbdOdomScan3dInfo),
-		SYNC_INIT(rgbdOdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// 1 RGBD + User Data
@@ -161,9 +155,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbdDataScan3d),
 		SYNC_INIT(rgbdDataScanDesc),
 		SYNC_INIT(rgbdDataInfo),
-		SYNC_INIT(rgbdDataScan2dInfo),
-		SYNC_INIT(rgbdDataScan3dInfo),
-		SYNC_INIT(rgbdDataScanDescInfo),
 
 		// 1 RGBD + Odom + User Data
 		SYNC_INIT(rgbdOdomData),
@@ -171,9 +162,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbdOdomDataScan3d),
 		SYNC_INIT(rgbdOdomDataScanDesc),
 		SYNC_INIT(rgbdOdomDataInfo),
-		SYNC_INIT(rgbdOdomDataScan2dInfo),
-		SYNC_INIT(rgbdOdomDataScan3dInfo),
-		SYNC_INIT(rgbdOdomDataScanDescInfo),
 #endif
 
 #ifdef RTABMAP_SYNC_MULTI_RGBD
@@ -183,9 +171,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd2Scan3d),
 		SYNC_INIT(rgbd2ScanDesc),
 		SYNC_INIT(rgbd2Info),
-		SYNC_INIT(rgbd2Scan2dInfo),
-		SYNC_INIT(rgbd2Scan3dInfo),
-		SYNC_INIT(rgbd2ScanDescInfo),
 
 		// 2 RGBD + Odom
 		SYNC_INIT(rgbd2Odom),
@@ -193,9 +178,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd2OdomScan3d),
 		SYNC_INIT(rgbd2OdomScanDesc),
 		SYNC_INIT(rgbd2OdomInfo),
-		SYNC_INIT(rgbd2OdomScan2dInfo),
-		SYNC_INIT(rgbd2OdomScan3dInfo),
-		SYNC_INIT(rgbd2OdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// 2 RGBD + User Data
@@ -204,9 +186,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd2DataScan3d),
 		SYNC_INIT(rgbd2DataScanDesc),
 		SYNC_INIT(rgbd2DataInfo),
-		SYNC_INIT(rgbd2DataScan2dInfo),
-		SYNC_INIT(rgbd2DataScan3dInfo),
-		SYNC_INIT(rgbd2DataScanDescInfo),
 
 		// 2 RGBD + Odom + User Data
 		SYNC_INIT(rgbd2OdomData),
@@ -214,9 +193,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd2OdomDataScan3d),
 		SYNC_INIT(rgbd2OdomDataScanDesc),
 		SYNC_INIT(rgbd2OdomDataInfo),
-		SYNC_INIT(rgbd2OdomDataScan2dInfo),
-		SYNC_INIT(rgbd2OdomDataScan3dInfo),
-		SYNC_INIT(rgbd2OdomDataScanDescInfo),
 #endif
 
 		// 3 RGBD
@@ -225,9 +201,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd3Scan3d),
 		SYNC_INIT(rgbd3ScanDesc),
 		SYNC_INIT(rgbd3Info),
-		SYNC_INIT(rgbd3Scan2dInfo),
-		SYNC_INIT(rgbd3Scan3dInfo),
-		SYNC_INIT(rgbd3ScanDescInfo),
 
 		// 3 RGBD + Odom
 		SYNC_INIT(rgbd3Odom),
@@ -235,9 +208,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd3OdomScan3d),
 		SYNC_INIT(rgbd3OdomScanDesc),
 		SYNC_INIT(rgbd3OdomInfo),
-		SYNC_INIT(rgbd3OdomScan2dInfo),
-		SYNC_INIT(rgbd3OdomScan3dInfo),
-		SYNC_INIT(rgbd3OdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// 3 RGBD + User Data
@@ -246,9 +216,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd3DataScan3d),
 		SYNC_INIT(rgbd3DataScanDesc),
 		SYNC_INIT(rgbd3DataInfo),
-		SYNC_INIT(rgbd3DataScan2dInfo),
-		SYNC_INIT(rgbd3DataScan3dInfo),
-		SYNC_INIT(rgbd3DataScanDescInfo),
 
 		// 3 RGBD + Odom + User Data
 		SYNC_INIT(rgbd3OdomData),
@@ -256,9 +223,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd3OdomDataScan3d),
 		SYNC_INIT(rgbd3OdomDataScanDesc),
 		SYNC_INIT(rgbd3OdomDataInfo),
-		SYNC_INIT(rgbd3OdomDataScan2dInfo),
-		SYNC_INIT(rgbd3OdomDataScan3dInfo),
-		SYNC_INIT(rgbd3OdomDataScanDescInfo),
 #endif
 
 		// 4 RGBD
@@ -267,9 +231,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd4Scan3d),
 		SYNC_INIT(rgbd4ScanDesc),
 		SYNC_INIT(rgbd4Info),
-		SYNC_INIT(rgbd4Scan2dInfo),
-		SYNC_INIT(rgbd4Scan3dInfo),
-		SYNC_INIT(rgbd4ScanDescInfo),
 
 		// 4 RGBD + Odom
 		SYNC_INIT(rgbd4Odom),
@@ -277,9 +238,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd4OdomScan3d),
 		SYNC_INIT(rgbd4OdomScanDesc),
 		SYNC_INIT(rgbd4OdomInfo),
-		SYNC_INIT(rgbd4OdomScan2dInfo),
-		SYNC_INIT(rgbd4OdomScan3dInfo),
-		SYNC_INIT(rgbd4OdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// 4 RGBD + User Data
@@ -288,9 +246,6 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd4DataScan3d),
 		SYNC_INIT(rgbd4DataScanDesc),
 		SYNC_INIT(rgbd4DataInfo),
-		SYNC_INIT(rgbd4DataScan2dInfo),
-		SYNC_INIT(rgbd4DataScan3dInfo),
-		SYNC_INIT(rgbd4DataScanDescInfo),
 
 		// 4 RGBD + Odom + User Data
 		SYNC_INIT(rgbd4OdomData),
@@ -298,10 +253,34 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd4OdomDataScan3d),
 		SYNC_INIT(rgbd4OdomDataScanDesc),
 		SYNC_INIT(rgbd4OdomDataInfo),
-		SYNC_INIT(rgbd4OdomDataScan2dInfo),
-		SYNC_INIT(rgbd4OdomDataScan3dInfo),
-		SYNC_INIT(rgbd4OdomDataScanDescInfo),
 #endif
+		// 5 RGBD
+		SYNC_INIT(rgbd5),
+		SYNC_INIT(rgbd5Scan2d),
+		SYNC_INIT(rgbd5Scan3d),
+		SYNC_INIT(rgbd5ScanDesc),
+		SYNC_INIT(rgbd5Info),
+
+		// 5 RGBD + Odom
+		SYNC_INIT(rgbd5Odom),
+		SYNC_INIT(rgbd5OdomScan2d),
+		SYNC_INIT(rgbd5OdomScan3d),
+		SYNC_INIT(rgbd5OdomScanDesc),
+		SYNC_INIT(rgbd5OdomInfo),
+
+		// 6 RGBD
+		SYNC_INIT(rgbd6),
+		SYNC_INIT(rgbd6Scan2d),
+		SYNC_INIT(rgbd6Scan3d),
+		SYNC_INIT(rgbd6ScanDesc),
+		SYNC_INIT(rgbd6Info),
+
+		// 6 RGBD + Odom
+		SYNC_INIT(rgbd6Odom),
+		SYNC_INIT(rgbd6OdomScan2d),
+		SYNC_INIT(rgbd6OdomScan3d),
+		SYNC_INIT(rgbd6OdomScanDesc),
+		SYNC_INIT(rgbd6OdomInfo),
 #endif // RTABMAP_SYNC_MULTI_RGBD
 
 		// Scan
@@ -518,7 +497,35 @@ void CommonDataSubscriber::setupCallbacks(
 	else if(subscribedToRGBD_)
 	{
 #ifdef RTABMAP_SYNC_MULTI_RGBD
-		if(rgbdCameras == 4)
+		if(rgbdCameras == 6)
+		{
+			setupRGBD6Callbacks(
+					nh,
+					pnh,
+					subscribedToOdom_,
+					subscribeUserData,
+					subscribeScan2d,
+					subscribeScan3d,
+					subscribeScanDesc,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync_);
+		}
+		else if(rgbdCameras == 5)
+		{
+			setupRGBD5Callbacks(
+					nh,
+					pnh,
+					subscribedToOdom_,
+					subscribeUserData,
+					subscribeScan2d,
+					subscribeScan3d,
+					subscribeScanDesc,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync_);
+		}
+		else if(rgbdCameras == 4)
 		{
 			setupRGBD4Callbacks(
 					nh,
@@ -718,9 +725,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbdScan3d);
 	SYNC_DEL(rgbdScanDesc);
 	SYNC_DEL(rgbdInfo);
-	SYNC_DEL(rgbdScan2dInfo);
-	SYNC_DEL(rgbdScan3dInfo);
-	SYNC_DEL(rgbdScanDescInfo);
 
 	// 1 RGBD + Odom
 	SYNC_DEL(rgbdOdom);
@@ -728,9 +732,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbdOdomScan3d);
 	SYNC_DEL(rgbdOdomScanDesc);
 	SYNC_DEL(rgbdOdomInfo);
-	SYNC_DEL(rgbdOdomScan2dInfo);
-	SYNC_DEL(rgbdOdomScan3dInfo);
-	SYNC_DEL(rgbdOdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 1 RGBD + User Data
@@ -739,9 +740,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbdDataScan3d);
 	SYNC_DEL(rgbdDataScanDesc);
 	SYNC_DEL(rgbdDataInfo);
-	SYNC_DEL(rgbdDataScan2dInfo);
-	SYNC_DEL(rgbdDataScan3dInfo);
-	SYNC_DEL(rgbdDataScanDescInfo);
 
 	// 1 RGBD + Odom + User Data
 	SYNC_DEL(rgbdOdomData);
@@ -749,9 +747,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbdOdomDataScan3d);
 	SYNC_DEL(rgbdOdomDataScanDesc);
 	SYNC_DEL(rgbdOdomDataInfo);
-	SYNC_DEL(rgbdOdomDataScan2dInfo);
-	SYNC_DEL(rgbdOdomDataScan3dInfo);
-	SYNC_DEL(rgbdOdomDataScanDescInfo);
 #endif
 
 #ifdef RTABMAP_SYNC_MULTI_RGBD
@@ -761,9 +756,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd2Scan3d);
 	SYNC_DEL(rgbd2ScanDesc);
 	SYNC_DEL(rgbd2Info);
-	SYNC_DEL(rgbd2Scan2dInfo);
-	SYNC_DEL(rgbd2Scan3dInfo);
-	SYNC_DEL(rgbd2ScanDescInfo);
 
 	// 2 RGBD + Odom
 	SYNC_DEL(rgbd2Odom);
@@ -771,9 +763,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd2OdomScan3d);
 	SYNC_DEL(rgbd2OdomScanDesc);
 	SYNC_DEL(rgbd2OdomInfo);
-	SYNC_DEL(rgbd2OdomScan2dInfo);
-	SYNC_DEL(rgbd2OdomScan3dInfo);
-	SYNC_DEL(rgbd2OdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 2 RGBD + User Data
@@ -782,9 +771,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd2DataScan3d);
 	SYNC_DEL(rgbd2DataScanDesc);
 	SYNC_DEL(rgbd2DataInfo);
-	SYNC_DEL(rgbd2DataScan2dInfo);
-	SYNC_DEL(rgbd2DataScan3dInfo);
-	SYNC_DEL(rgbd2DataScanDescInfo);
 
 	// 2 RGBD + Odom + User Data
 	SYNC_DEL(rgbd2OdomData);
@@ -792,9 +778,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd2OdomDataScan3d);
 	SYNC_DEL(rgbd2OdomDataScanDesc);
 	SYNC_DEL(rgbd2OdomDataInfo);
-	SYNC_DEL(rgbd2OdomDataScan2dInfo);
-	SYNC_DEL(rgbd2OdomDataScan3dInfo);
-	SYNC_DEL(rgbd2OdomDataScanDescInfo);
 #endif
 
 	// 3 RGBD
@@ -803,9 +786,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd3Scan3d);
 	SYNC_DEL(rgbd3ScanDesc);
 	SYNC_DEL(rgbd3Info);
-	SYNC_DEL(rgbd3Scan2dInfo);
-	SYNC_DEL(rgbd3Scan3dInfo);
-	SYNC_DEL(rgbd3ScanDescInfo);
 
 	// 3 RGBD + Odom
 	SYNC_DEL(rgbd3Odom);
@@ -813,9 +793,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd3OdomScan3d);
 	SYNC_DEL(rgbd3OdomScanDesc);
 	SYNC_DEL(rgbd3OdomInfo);
-	SYNC_DEL(rgbd3OdomScan2dInfo);
-	SYNC_DEL(rgbd3OdomScan3dInfo);
-	SYNC_DEL(rgbd3OdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 3 RGBD + User Data
@@ -824,9 +801,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd3DataScan3d);
 	SYNC_DEL(rgbd3DataScanDesc);
 	SYNC_DEL(rgbd3DataInfo);
-	SYNC_DEL(rgbd3DataScan2dInfo);
-	SYNC_DEL(rgbd3DataScan3dInfo);
-	SYNC_DEL(rgbd3DataScanDescInfo);
 
 	// 3 RGBD + Odom + User Data
 	SYNC_DEL(rgbd3OdomData);
@@ -834,9 +808,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd3OdomDataScan3d);
 	SYNC_DEL(rgbd3OdomDataScanDesc);
 	SYNC_DEL(rgbd3OdomDataInfo);
-	SYNC_DEL(rgbd3OdomDataScan2dInfo);
-	SYNC_DEL(rgbd3OdomDataScan3dInfo);
-	SYNC_DEL(rgbd3OdomDataScanDescInfo);
 #endif
 
 	// 4 RGBD
@@ -845,9 +816,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd4Scan3d);
 	SYNC_DEL(rgbd4ScanDesc);
 	SYNC_DEL(rgbd4Info);
-	SYNC_DEL(rgbd4Scan2dInfo);
-	SYNC_DEL(rgbd4Scan3dInfo);
-	SYNC_DEL(rgbd4ScanDescInfo);
 
 	// 4 RGBD + Odom
 	SYNC_DEL(rgbd4Odom);
@@ -855,9 +823,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd4OdomScan3d);
 	SYNC_DEL(rgbd4OdomScanDesc);
 	SYNC_DEL(rgbd4OdomInfo);
-	SYNC_DEL(rgbd4OdomScan2dInfo);
-	SYNC_DEL(rgbd4OdomScan3dInfo);
-	SYNC_DEL(rgbd4OdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 4 RGBD + User Data
@@ -866,9 +831,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd4DataScan3d);
 	SYNC_DEL(rgbd4DataScanDesc);
 	SYNC_DEL(rgbd4DataInfo);
-	SYNC_DEL(rgbd4DataScan2dInfo);
-	SYNC_DEL(rgbd4DataScan3dInfo);
-	SYNC_DEL(rgbd4DataScanDescInfo);
 
 	// 4 RGBD + Odom + User Data
 	SYNC_DEL(rgbd4OdomData);
@@ -876,10 +838,34 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd4OdomDataScan3d);
 	SYNC_DEL(rgbd4OdomDataScanDesc);
 	SYNC_DEL(rgbd4OdomDataInfo);
-	SYNC_DEL(rgbd4OdomDataScan2dInfo);
-	SYNC_DEL(rgbd4OdomDataScan3dInfo);
-	SYNC_DEL(rgbd4OdomDataScanDescInfo);
 #endif
+	// 5 RGBD
+	SYNC_DEL(rgbd5);
+	SYNC_DEL(rgbd5Scan2d);
+	SYNC_DEL(rgbd5Scan3d);
+	SYNC_DEL(rgbd5ScanDesc);
+	SYNC_DEL(rgbd5Info);
+
+	// 5 RGBD + Odom
+	SYNC_DEL(rgbd5Odom);
+	SYNC_DEL(rgbd5OdomScan2d);
+	SYNC_DEL(rgbd5OdomScan3d);
+	SYNC_DEL(rgbd5OdomScanDesc);
+	SYNC_DEL(rgbd5OdomInfo);
+
+	// 6 RGBD
+	SYNC_DEL(rgbd6);
+	SYNC_DEL(rgbd6Scan2d);
+	SYNC_DEL(rgbd6Scan3d);
+	SYNC_DEL(rgbd6ScanDesc);
+	SYNC_DEL(rgbd6Info);
+
+	// 6 RGBD + Odom
+	SYNC_DEL(rgbd6Odom);
+	SYNC_DEL(rgbd6OdomScan2d);
+	SYNC_DEL(rgbd6OdomScan3d);
+	SYNC_DEL(rgbd6OdomScanDesc);
+	SYNC_DEL(rgbd6OdomInfo);
 #endif //RTABMAP_SYNC_MULTI_RGBD
 
 	// Scan

src/MsgConversion.cpp

@@ -145,11 +145,6 @@ void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb,
 		rgb = cv_bridge::toCvCopy(image.rgb_compressed);
 #endif
 	}
-	else
-	{
-		// empty
-		rgb = boost::make_shared<cv_bridge::CvImage>();
-	}
 
 	if(!image.depth.data.empty())
 	{
@@ -164,11 +159,6 @@ void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb,
 		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)
@@ -185,11 +175,6 @@ void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageC
 		rgb = cv_bridge::toCvCopy(image->rgb_compressed);
 #endif
 	}
-	else
-	{
-		// empty
-		rgb = boost::make_shared<cv_bridge::CvImage>();
-	}
 
 	if(!image->depth.data.empty())
 	{
@@ -215,11 +200,6 @@ void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageC
 			depth = ptr;
 		}
 	}
-	else
-	{
-		// empty
-		depth = boost::make_shared<cv_bridge::CvImage>();
-	}
 }
 
 rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image)
@@ -1626,7 +1606,8 @@ bool convertRGBDMsgs(
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
-			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0))
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_RGGB8) == 0))
 		{
 
 			ROS_ERROR("Input rgb type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb=%s",

src/RGBSyncNode.cpp

@@ -0,0 +1,47 @@
+/*
+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 "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgb_sync");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/rgb_sync", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/StereoSyncNode.cpp

@@ -0,0 +1,47 @@
+/*
+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 "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "stereo_sync");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/stereo_sync", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/impl/CommonDataSubscriberRGBD.cpp

@@ -153,77 +153,6 @@ void CommonDataSubscriber::rgbdInfoCallback(
 			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
 			rtabmap::uncompressData(image1Msg->descriptors));
 }
-void CommonDataSubscriber::rgbdScan2dInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		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::PointCloud2 scan3dMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			*scanMsg, scan3dMsg, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
-void CommonDataSubscriber::rgbdScan3dInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		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::LaserScan scanMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			scanMsg, *scan3dMsg, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
-void CommonDataSubscriber::rgbdScanDescInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		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
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
 
 // 1 RGBD camera + Odom
 void CommonDataSubscriber::rgbdOdomCallback(
@@ -349,92 +278,6 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
 			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
 			rtabmap::uncompressData(image1Msg->descriptors));
 }
-void CommonDataSubscriber::rgbdOdomScan2dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			*scanMsg, scan3dMsg, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
-void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-
-	std::vector<double> current_feature_vector;
-	    std::vector<int> lengths_feature_vector;
-
-	rtabmap_ros::ScanDescriptor scanDescriptor;
-	scanDescriptor.header = scan3dMsg->header;
-	scanDescriptor.scan_cloud = *scan3dMsg;
-	scanDescriptor.global_descriptor.type=0;
-	scanDescriptor.global_descriptor.info=rtabmap::compressData(cv::Mat(1, lengths_feature_vector.size(), CV_32FC1, (void*)lengths_feature_vector.data()));
-	scanDescriptor.global_descriptor.data=rtabmap::compressData(cv::Mat(1, current_feature_vector.size(), CV_64FC1, (void*)current_feature_vector.data()));
-
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			scanMsg, *scan3dMsg, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
-void CommonDataSubscriber::rgbdOdomScanDescInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
 
 #ifdef RTABMAP_SYNC_USER_DATA
 // 1 RGBD camera + User Data
@@ -561,82 +404,6 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
 			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
 			rtabmap::uncompressData(image1Msg->descriptors));
 }
-void CommonDataSubscriber::rgbdDataScan2dInfoCallback(
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			*scanMsg, scan3dMsg, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
-void CommonDataSubscriber::rgbdDataScan3dInfoCallback(
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			scanMsg, *scan3dMsg, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
-void CommonDataSubscriber::rgbdDataScanDescInfoCallback(
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
 
 // 1 RGBD camera + Odom + User Data
 void CommonDataSubscriber::rgbdOdomDataCallback(
@@ -762,80 +529,6 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
 			rtabmap::uncompressData(image1Msg->descriptors));
 }
-void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			*scanMsg, scan3dMsg, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
-void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	sensor_msgs::LaserScan scanMsg; // Null
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			scanMsg, *scan3dMsg, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
-void CommonDataSubscriber::rgbdOdomDataScanDescInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
-	if(!image1Msg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
-	}
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
-			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
-			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
-			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
-			rtabmap::uncompressData(image1Msg->descriptors));
-}
 #endif
 
 void CommonDataSubscriber::setupRGBDCallbacks(
@@ -869,14 +562,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL5(rgbdOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL4(rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
 				}
+				SYNC_DECL4(rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
 			}
 			else if(subscribeScan2d)
 			{
@@ -884,14 +573,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scanSub_.subscribe(nh, "scan", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL5(rgbdOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
 				}
+				SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
@@ -899,14 +584,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL5(rgbdOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
 				}
+				SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
@@ -930,14 +611,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL4(rgbdOdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
 				}
+				SYNC_DECL3(rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
 			}
 			else if(subscribeScan2d)
 			{
@@ -945,14 +622,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scanSub_.subscribe(nh, "scan", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL4(rgbdOdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
 				}
+				SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
@@ -960,14 +633,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL4(rgbdOdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
 				}
+				SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
@@ -990,14 +659,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL4(rgbdDataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
 				}
+				SYNC_DECL3(rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
 			}
 			else if(subscribeScan2d)
 			{
@@ -1005,14 +670,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scanSub_.subscribe(nh, "scan", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL4(rgbdDataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
 				}
+				SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
@@ -1020,14 +681,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL4(rgbdDataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
 				}
+				SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
@@ -1049,14 +706,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL3(rgbdScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL2(rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
 				}
+				SYNC_DECL2(rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
 			}
 			else if(subscribeScan2d)
 			{
@@ -1064,14 +717,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scanSub_.subscribe(nh, "scan", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL3(rgbdScan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
 				}
+				SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
@@ -1079,14 +728,10 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 				scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
+					subscribedToOdomInfo_ = false;
-					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					ROS_WARN("subscribe_odom_info ignored...");
-					SYNC_DECL3(rgbdScan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
 				}
+				SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{

src/impl/CommonDataSubscriberRGBD2.cpp

@@ -39,6 +39,8 @@ namespace rtabmap_ros {
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
 		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
 		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
 		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
@@ -126,49 +128,6 @@ void CommonDataSubscriber::rgbd2InfoCallback(
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd2Scan2dInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd2Scan3dInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd2ScanDescInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-
 // 2 RGBD + Odom
 void CommonDataSubscriber::rgbd2OdomCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -238,48 +197,6 @@ void CommonDataSubscriber::rgbd2OdomInfoCallback(
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd2OdomScan2dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd2OdomScan3dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd2OdomScanDescInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
 
 #ifdef RTABMAP_SYNC_USER_DATA
 // 2 RGBD + User Data
@@ -351,48 +268,6 @@ void CommonDataSubscriber::rgbd2DataInfoCallback(
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd2DataScan2dInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd2DataScan3dInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd2DataScanDescInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
 
 // 2 RGBD + Odom + User Data
 void CommonDataSubscriber::rgbd2OdomDataCallback(
@@ -463,47 +338,6 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd2OdomDataScan2dInfoCallback(
-		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,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd2OdomDataScan3dInfoCallback(
-		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,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd2OdomDataScanDescInfoCallback(
-		const nav_msgs::OdometryConstPtr& odomMsg,
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
 #endif
 
 void CommonDataSubscriber::setupRGBD2Callbacks(
@@ -537,14 +371,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd2OdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 			}
+			SYNC_DECL5(rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -552,14 +382,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd2OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 			}
+			SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -567,14 +393,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd2OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 			}
+			SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -598,14 +420,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd2OdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 			}
+			SYNC_DECL4(rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -613,14 +431,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd2OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 			}
+			SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -628,14 +442,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd2OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 			}
+			SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -658,14 +468,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd2DataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 			}
+			SYNC_DECL4(rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -673,14 +479,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd2DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 			}
+			SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -688,14 +490,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd2DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 			}
+			SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -717,14 +515,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL4(rgbd2ScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL3(rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 			}
+			SYNC_DECL3(rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -732,14 +526,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL4(rgbd2Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 			}
+			SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -747,14 +537,10 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL4(rgbd2Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 			}
+			SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{

src/impl/CommonDataSubscriberRGBD3.cpp

@@ -40,6 +40,8 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
 		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
 		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
 		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
@@ -153,60 +155,6 @@ void CommonDataSubscriber::rgbd3InfoCallback(
 			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
 			localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd3Scan2dInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, *scanMsg,
-			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd3Scan3dInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, scanMsg,
-			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd3ScanDescInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
-			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
 
 // 2 RGBD + Odom
 void CommonDataSubscriber::rgbd3OdomCallback(
@@ -297,61 +245,6 @@ void CommonDataSubscriber::rgbd3OdomInfoCallback(
 			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
 			localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd3OdomScan2dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, *scanMsg,
-			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd3OdomScan3dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, scanMsg,
-			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd3OdomScanDescInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
-			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
 
 #ifdef RTABMAP_SYNC_USER_DATA
 // 2 RGBD + User Data
@@ -443,61 +336,6 @@ void CommonDataSubscriber::rgbd3DataInfoCallback(
 			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
 			localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd3DataScan2dInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, *scanMsg,
-			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd3DataScan3dInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, scanMsg,
-			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd3DataScanDescInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
-			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-
 // 2 RGBD + Odom + User Data
 void CommonDataSubscriber::rgbd3OdomDataCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -587,60 +425,6 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
 			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
 			localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd3OdomDataScan2dInfoCallback(
-		const nav_msgs::OdometryConstPtr& odomMsg,
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, *scanMsg,
-			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd3OdomDataScan3dInfoCallback(
-		const nav_msgs::OdometryConstPtr& odomMsg,
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, scanMsg,
-			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd3OdomDataScanDescInfoCallback(
-		const nav_msgs::OdometryConstPtr& odomMsg,
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
-			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
-			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
-			localKeyPoints, localPoints3d, localDescriptors);
-}
 #endif
 
 void CommonDataSubscriber::setupRGBD3Callbacks(
@@ -674,14 +458,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd3OdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 			}
+			SYNC_DECL6(rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -689,14 +469,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd3OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 			}
+			SYNC_DECL6(rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -704,14 +480,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd3OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 			}
+			SYNC_DECL6(rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -735,14 +507,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd3OdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 			}
+			SYNC_DECL5(rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -750,14 +518,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd3OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 			}
+			SYNC_DECL5(rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -765,14 +529,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd3OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 			}
+			SYNC_DECL5(rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -795,29 +555,20 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd3DataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_, odomInfoSub_);
 			}
-			else
+			SYNC_DECL5(rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);		}
-			{
-				SYNC_DECL5(rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
-			}
-		}
 		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd3DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 			}
+			SYNC_DECL5(rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -825,14 +576,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd3DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 			}
+			SYNC_DECL5(rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -854,14 +601,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd3ScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 			}
+			SYNC_DECL4(rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -869,14 +612,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd3Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 			}
+			SYNC_DECL4(rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -884,14 +623,10 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL5(rgbd3Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 			}
+			SYNC_DECL4(rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{

src/impl/CommonDataSubscriberRGBD4.cpp

@@ -41,6 +41,8 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
 		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
 		rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
 		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
 		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
@@ -150,54 +152,6 @@ void CommonDataSubscriber::rgbd4InfoCallback(
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd4Scan2dInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd4Scan3dInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd4ScanDescInfoCallback(
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
 
 // 2 RGBD + Odom
 void CommonDataSubscriber::rgbd4OdomCallback(
@@ -278,54 +232,6 @@ void CommonDataSubscriber::rgbd4OdomInfoCallback(
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd4OdomScan2dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd4OdomScan3dInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd4OdomScanDescInfoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
 
 #ifdef RTABMAP_SYNC_USER_DATA
 // 2 RGBD + User Data
@@ -407,54 +313,6 @@ void CommonDataSubscriber::rgbd4DataInfoCallback(
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd4DataScan2dInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd4DataScan3dInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd4DataScanDescInfoCallback(
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
 
 // 2 RGBD + Odom + User Data
 void CommonDataSubscriber::rgbd4OdomDataCallback(
@@ -535,54 +393,6 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd4OdomDataScan2dInfoCallback(
-		const nav_msgs::OdometryConstPtr& odomMsg,
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd4OdomDataScan3dInfoCallback(
-		const nav_msgs::OdometryConstPtr& odomMsg,
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
-void CommonDataSubscriber::rgbd4OdomDataScanDescInfoCallback(
-		const nav_msgs::OdometryConstPtr& odomMsg,
-		const rtabmap_ros::UserDataConstPtr& userDataMsg,
-		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
-		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
-		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	if(!scanDescMsg->global_descriptor.data.empty())
-	{
-		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
-	}
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
-}
 #endif
 
 void CommonDataSubscriber::setupRGBD4Callbacks(
@@ -616,14 +426,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL8(rgbd4OdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL7(rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 			}
+			SYNC_DECL7(rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -631,14 +437,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL8(rgbd4OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL7(rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 			}
+			SYNC_DECL7(rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -646,14 +448,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL8(rgbd4OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL7(rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 			}
+			SYNC_DECL7(rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -677,14 +475,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd4OdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 			}
+			SYNC_DECL6(rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -692,14 +486,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd4OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 			}
+			SYNC_DECL6(rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -707,14 +497,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd4OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 			}
+			SYNC_DECL6(rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -737,14 +523,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd4DataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 			}
+			SYNC_DECL6(rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -752,14 +534,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd4DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 			}
+			SYNC_DECL6(rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -767,14 +545,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL7(rgbd4DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 			}
+			SYNC_DECL6(rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -796,14 +570,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd4ScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 			}
+			SYNC_DECL5(rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -811,14 +581,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scanSub_.subscribe(nh, "scan", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd4Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 			}
+			SYNC_DECL5(rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -826,14 +592,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
-				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				ROS_WARN("subscribe_odom_info ignored...");
-				SYNC_DECL6(rgbd4Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 			}
+			SYNC_DECL5(rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{

src/impl/CommonDataSubscriberRGBD5.cpp

@@ -0,0 +1,368 @@
+/*
+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/core/Compression.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5); \
+		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		rtabmap_ros::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		if(!image3Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
+		if(!image4Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
+		if(!image5Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image5Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localKeyPoints.push_back(image3Msg->key_points); \
+		localKeyPoints.push_back(image4Msg->key_points); \
+		localKeyPoints.push_back(image5Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localPoints3d.push_back(image4Msg->points); \
+		localPoints3d.push_back(image5Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors));
+
+// 5 RGBD
+void CommonDataSubscriber::rgbd5Callback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5Scan2dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5Scan3dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5ScanDescCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5InfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 5 RGBD + Odom
+void CommonDataSubscriber::rgbd5OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+void CommonDataSubscriber::setupRGBD5Callbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd5 callback");
+
+	rgbdSubs_.resize(5);
+	for(int i=0; i<5; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+	}
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL7(rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL6(rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+		}
+	}
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL6(rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGBD6.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/core/Compression.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6); \
+		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		rtabmap_ros::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
+		rtabmap_ros::toCvShare(image6Msg, imageMsgs[5], depthMsgs[5]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image6Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		if(!image3Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
+		if(!image4Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
+		if(!image5Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image5Msg->global_descriptor); \
+		if(!image6Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image6Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localKeyPoints.push_back(image3Msg->key_points); \
+		localKeyPoints.push_back(image4Msg->key_points); \
+		localKeyPoints.push_back(image5Msg->key_points); \
+		localKeyPoints.push_back(image6Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localPoints3d.push_back(image4Msg->points); \
+		localPoints3d.push_back(image5Msg->points); \
+		localPoints3d.push_back(image6Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image6Msg->descriptors));
+
+// 6 RGBD
+void CommonDataSubscriber::rgbd6Callback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6Scan2dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6Scan3dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6ScanDescCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6InfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 6 RGBD + Odom
+void CommonDataSubscriber::rgbd6OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image5Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image6Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+void CommonDataSubscriber::setupRGBD6Callbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd6 callback");
+
+	rgbdSubs_.resize(6);
+	for(int i=0; i<6; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+	}
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL8(rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL7(rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+		}
+	}
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL7(rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL6(rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/nodelets/point_cloud_assembler.cpp

@@ -81,7 +81,8 @@ public:
 		voxelSize_(0),
 		noiseRadius_(0),
 		noiseMinNeighbors_(5),
-		fixedFrameId_("odom")
+		fixedFrameId_("odom"),
+		frameId_("")
 	{}
 
 	virtual ~PointCloudAssembler()
@@ -108,6 +109,7 @@ private:
 
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+		pnh.param("frame_id", frameId_, frameId_);
 		pnh.param("max_clouds", maxClouds_, maxClouds_);
 		pnh.param("assembling_time", assemblingTime_, assemblingTime_);
 		pnh.param("skip_clouds", skipClouds_, skipClouds_);
@@ -123,6 +125,7 @@ private:
 
 		ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize);
 		ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str());
+		ROS_INFO("%s: frame_id=%s", getName().c_str(), frameId_.c_str());
 		ROS_INFO("%s: max_clouds=%d", getName().c_str(), maxClouds_);
 		ROS_INFO("%s: assembling_time=%fs", getName().c_str(), assemblingTime_);
 		ROS_INFO("%s: skip_clouds=%d", getName().c_str(), skipClouds_);
@@ -139,7 +142,7 @@ private:
 		std::string subscribedTopicsMsg;
 		if(!fixedFrameId_.empty())
 		{
-			cloudSub_ = nh.subscribe("cloud", 1, &PointCloudAssembler::callbackCloud, this);
+			cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this);
 			subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
 								getName().c_str(),
 								cloudSub_.getTopic().c_str());
@@ -322,9 +325,29 @@ private:
 					}
 
 					pcl_conversions::moveFromPCL(*assembled, rosCloud);
+					if(!frameId_.empty())
+					{
+						// transform in target frame_id instead of sensor frame
+						t = rtabmap_ros::getTransform(
+								fixedFrameId_, //fromFrame
+								frameId_, //toFrame
+								cloudMsg->header.stamp,
+								tfListener_,
+								waitForTransformDuration_);
+						if(t.isNull())
+						{
+							ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
+							clouds_.clear();
+							return;
+						}
+					}
 					pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
 
 					rosCloud.header = cloudMsg->header;
+					if(!frameId_.empty())
+					{
+						rosCloud.header.frame_id = frameId_;
+					}
 					cloudPub_.publish(rosCloud);
 					if(circularBuffer_)
 					{
@@ -390,6 +413,7 @@ private:
 	double noiseRadius_;
 	int noiseMinNeighbors_;
 	std::string fixedFrameId_;
+	std::string frameId_;
 	tf::TransformListener tfListener_;
 
 	std::list<pcl::PCLPointCloud2::Ptr> clouds_;

src/nodelets/rgb_sync.cpp

@@ -0,0 +1,227 @@
+/*
+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 <boost/thread.hpp>
+
+#include "rtabmap_ros/RGBDImage.h"
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_ros
+{
+
+class RgbSync : public nodelet::Nodelet
+{
+public:
+	RgbSync() :
+		compressedRate_(0),
+		warningThread_(0),
+		callbackCalled_(false),
+		approxSync_(0),
+		exactSync_(0)
+	{}
+
+	virtual ~RgbSync()
+	{
+		if(approxSync_)
+			delete approxSync_;
+		if(exactSync_)
+			delete exactSync_;
+
+		if(warningThread_)
+		{
+			callbackCalled_=true;
+			warningThread_->join();
+			delete warningThread_;
+		}
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = false;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("compressed_rate", compressedRate_, compressedRate_);
+
+		NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
+		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
+		NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
+
+		rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
+		rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
+			approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, _1, _2));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
+			exactSync_->registerCallback(boost::bind(&RgbSync::callback, this, _1, _2));
+		}
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), 1, hintsRgb);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s",
+							getName().c_str(),
+							approxSync?"approx":"exact",
+							imageSub_.getTopic().c_str(),
+							cameraInfoSub_.getTopic().c_str());
+
+		warningThread_ = new boost::thread(boost::bind(&RgbSync::warningLoop, this, subscribedTopicsMsg, approxSync));
+		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
+	}
+
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
+	{
+		ros::Duration r(5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						getName().c_str(),
+						approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+						subscribedTopicsMsg.c_str());
+			}
+		}
+	}
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& image,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		callbackCalled_ = true;
+		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
+		{
+			double stamp = image->header.stamp.toSec();
+
+			rtabmap_ros::RGBDImage msg;
+			msg.header.frame_id = cameraInfo->header.frame_id;
+			msg.header.stamp = image->header.stamp;
+			msg.rgb_camera_info = *cameraInfo;
+
+			if(rgbdImageCompressedPub_.getNumSubscribers())
+			{
+				bool publishCompressed = true;
+				if (compressedRate_ > 0.0)
+				{
+					if ( lastCompressedPublished_ + ros::Duration(1.0/compressedRate_) > ros::Time::now())
+					{
+						NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
+						publishCompressed = false;
+					}
+				}
+
+				if(publishCompressed)
+				{
+					lastCompressedPublished_ = ros::Time::now();
+
+					rtabmap_ros::RGBDImage msgCompressed = msg;
+
+					cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+					imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
+
+					rgbdImageCompressedPub_.publish(msgCompressed);
+				}
+			}
+
+			if(rgbdImagePub_.getNumSubscribers())
+			{
+				msg.rgb = *image;
+				rgbdImagePub_.publish(msg);
+			}
+
+			if( stamp != image->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"%f->%f",
+						stamp, image->header.stamp.toSec());
+			}
+		}
+	}
+
+private:
+	double compressedRate_;
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+	ros::Time lastCompressedPublished_;
+
+	ros::Publisher rgbdImagePub_;
+	ros::Publisher rgbdImageCompressedPub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RgbSync, nodelet::Nodelet);
+}
+