Added OdomInfo + scan2d/scan3d synchronization interface

src/impl/CommonDataSubscriberDepth.cpp

@@ -78,6 +78,30 @@ void CommonDataSubscriber::depthInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
+void CommonDataSubscriber::depthScan2dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan3dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
 
 // RGB + Depth + Odom
 void CommonDataSubscriber::depthOdomCallback(
@@ -128,6 +152,30 @@ void CommonDataSubscriber::depthOdomInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
+void CommonDataSubscriber::depthOdomScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
 
 // RGB + Depth + User Data
 void CommonDataSubscriber::depthDataCallback(
@@ -178,6 +226,30 @@ void CommonDataSubscriber::depthDataInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
+void CommonDataSubscriber::depthDataScan2dInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan3dInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
 
 // RGB + Depth + Odom + User Data
 void CommonDataSubscriber::depthOdomDataCallback(
@@ -228,6 +300,30 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
+void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
 
 void CommonDataSubscriber::setupDepthCallbacks(
 		ros::NodeHandle & nh,
@@ -266,13 +362,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
-			SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
 			scan3dSub_.subscribe(nh, "scan_cloud", 1);
-			SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			}
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -293,13 +407,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
-			SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
 			scan3dSub_.subscribe(nh, "scan_cloud", 1);
-			SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			}
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -320,13 +452,32 @@ void CommonDataSubscriber::setupDepthCallbacks(
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
-			SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
 			scan3dSub_.subscribe(nh, "scan_cloud", 1);
-			SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			}
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -345,13 +496,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
-			SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
 			scan3dSub_.subscribe(nh, "scan_cloud", 1);
-			SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			}
 		}
 		else if(subscribeOdomInfo)
 		{

src/impl/CommonDataSubscriberRGBD.cpp

@@ -85,6 +85,32 @@ void CommonDataSubscriber::rgbdInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
+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::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+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::LaserScanConstPtr scanMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
 
 // 1 RGBD camera + Odom
 void CommonDataSubscriber::rgbdOdomCallback(
@@ -139,6 +165,32 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
+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::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		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
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
 
 // 1 RGBD camera + User Data
 void CommonDataSubscriber::rgbdDataCallback(
@@ -193,6 +245,32 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
+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::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+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::LaserScanConstPtr scanMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
 
 // 1 RGBD camera + Odom + User Data
 void CommonDataSubscriber::rgbdOdomDataCallback(
@@ -247,6 +325,32 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
+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::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+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::LaserScanConstPtr scanMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
 
 void CommonDataSubscriber::setupRGBDCallbacks(
 		ros::NodeHandle & nh,
@@ -276,13 +380,31 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 			{
 				subscribedToScan2d_ = true;
 				scanSub_.subscribe(nh, "scan", 1);
-				SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL5(rgbdOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+				}
 			}
 			else if(subscribeScan3d)
 			{
 				subscribedToScan3d_ = true;
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
-				SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL5(rgbdOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+				}
 			}
 			else if(subscribeOdomInfo)
 			{
@@ -302,13 +424,31 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 			{
 				subscribedToScan2d_ = true;
 				scanSub_.subscribe(nh, "scan", 1);
-				SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(rgbdOdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
+				}
 			}
 			else if(subscribeScan3d)
 			{
 				subscribedToScan3d_ = true;
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
-				SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(rgbdOdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
+				}
 			}
 			else if(subscribeOdomInfo)
 			{
@@ -328,13 +468,31 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 			{
 				subscribedToScan2d_ = true;
 				scanSub_.subscribe(nh, "scan", 1);
-				SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(rgbdDataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+				}
 			}
 			else if(subscribeScan3d)
 			{
 				subscribedToScan3d_ = true;
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
-				SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(rgbdDataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+				}
 			}
 			else if(subscribeOdomInfo)
 			{
@@ -353,13 +511,31 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 			{
 				subscribedToScan2d_ = true;
 				scanSub_.subscribe(nh, "scan", 1);
-				SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(rgbdScan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
+				}
 			}
 			else if(subscribeScan3d)
 			{
 				subscribedToScan3d_ = true;
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
-				SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(rgbdScan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
+				}
 			}
 			else if(subscribeOdomInfo)
 			{

src/impl/CommonDataSubscriberRGBD2.cpp

@@ -95,6 +95,32 @@ void CommonDataSubscriber::rgbd2InfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
 }
+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::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+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::LaserScanConstPtr scanMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
 
 // 2 RGBD + Odom
 void CommonDataSubscriber::rgbd2OdomCallback(
@@ -149,6 +175,32 @@ void CommonDataSubscriber::rgbd2OdomInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
 }
+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::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+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::LaserScanConstPtr scanMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
 
 // 2 RGBD + User Data
 void CommonDataSubscriber::rgbd2DataCallback(
@@ -203,6 +255,32 @@ void CommonDataSubscriber::rgbd2DataInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
 }
+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::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+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::LaserScanConstPtr scanMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
 
 // 2 RGBD + Odom + User Data
 void CommonDataSubscriber::rgbd2OdomDataCallback(
@@ -257,6 +335,32 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
 }
+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::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+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::LaserScanConstPtr scanMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
 
 void CommonDataSubscriber::setupRGBD2Callbacks(
 		ros::NodeHandle & nh,
@@ -285,13 +389,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
-			SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				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_);
+			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
 			scan3dSub_.subscribe(nh, "scan_cloud", 1);
-			SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				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_);
+			}
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -311,13 +433,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
-			SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbd2OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
 			scan3dSub_.subscribe(nh, "scan_cloud", 1);
-			SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbd2OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			}
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -337,13 +477,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
-			SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbd2DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
 			scan3dSub_.subscribe(nh, "scan_cloud", 1);
-			SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbd2DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			}
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -362,13 +520,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
-			SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbd2Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
 			scan3dSub_.subscribe(nh, "scan_cloud", 1);
-			SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbd2Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			}
 		}
 		else if(subscribeOdomInfo)
 		{