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.

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