void CommonDataSubscriber::commonSingleDepthCallback(
		const nav_msgs::OdometryConstPtr & odomMsg,
		const rtabmap_ros::UserDataConstPtr & userDataMsg,
		const cv_bridge::CvImageConstPtr & imageMsg,
		const cv_bridge::CvImageConstPtr & depthMsg,
		const sensor_msgs::CameraInfo & cameraInfoMsg,
		const sensor_msgs::LaserScanConstPtr& scanMsg,
		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
	callbackCalled();
	std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
	std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
	std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
	if(imageMsg.get())
	{
		imageMsgs.push_back(imageMsg);
	}
	if(depthMsg.get())
	{
		depthMsgs.push_back(depthMsg);
	}
	cameraInfoMsgs.push_back(cameraInfoMsg);
	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::commonSingleDepthCallback(
		const nav_msgs::OdometryConstPtr & odomMsg,
		const rtabmap_ros::UserDataConstPtr & userDataMsg,
		const cv_bridge::CvImageConstPtr & imageMsg,
		const cv_bridge::CvImageConstPtr & depthMsg,
		const sensor_msgs::CameraInfo & rgbCameraInfoMsg,
		const sensor_msgs::CameraInfo & depthCameraInfoMsg,
		const sensor_msgs::LaserScanConstPtr& scanMsg,
		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
	callbackCalled();

	if(depthMsg.get() == 0 ||
	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
	   depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
	{
		std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
		std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
		if(imageMsg.get())
		{
			imageMsgs.push_back(imageMsg);
		}
		if(depthMsg.get())
		{
			depthMsgs.push_back(depthMsg);
		}
		cameraInfoMsgs.push_back(rgbCameraInfoMsg);
		commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
	}
	else // assuming stereo
	{
		commonStereoCallback(odomMsg, userDataMsg, imageMsg, depthMsg, rgbCameraInfoMsg, depthCameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
	}
}
CommonDataSubscriber::~CommonDataSubscriber()
{
	if(warningThread_)
	{
		callbackCalled();
		warningThread_->join();
		delete warningThread_;
	}

	// RGB + Depth
	SYNC_DEL(depth);
	SYNC_DEL(depthScan2d);
	SYNC_DEL(depthScan3d);
	SYNC_DEL(depthInfo);
	SYNC_DEL(depthScan2dInfo);
	SYNC_DEL(depthScan3dInfo);

	// RGB + Depth + Odom
	SYNC_DEL(depthOdom);
	SYNC_DEL(depthOdomScan2d);
	SYNC_DEL(depthOdomScan3d);
	SYNC_DEL(depthOdomInfo);
	SYNC_DEL(depthOdomScan2dInfo);
	SYNC_DEL(depthOdomScan3dInfo);

	// RGB + Depth + User Data
	SYNC_DEL(depthData);
	SYNC_DEL(depthDataScan2d);
	SYNC_DEL(depthDataScan3d);
	SYNC_DEL(depthDataInfo);
	SYNC_DEL(depthDataScan2dInfo);
	SYNC_DEL(depthDataScan3dInfo);

	// RGB + Depth + Odom + User Data
	SYNC_DEL(depthOdomData);
	SYNC_DEL(depthOdomDataScan2d);
	SYNC_DEL(depthOdomDataScan3d);
	SYNC_DEL(depthOdomDataInfo);
	SYNC_DEL(depthOdomDataScan2dInfo);
	SYNC_DEL(depthOdomDataScan3dInfo);

	// Stereo
	SYNC_DEL(stereo);
	SYNC_DEL(stereoInfo);

	// Stereo + Odom
	SYNC_DEL(stereoOdom);
	SYNC_DEL(stereoOdomInfo);

	// RGB-only
	SYNC_DEL(rgb);
	SYNC_DEL(rgbScan2d);
	SYNC_DEL(rgbScan3d);
	SYNC_DEL(rgbInfo);
	SYNC_DEL(rgbScan2dInfo);
	SYNC_DEL(rgbScan3dInfo);

	// RGB-only + Odom
	SYNC_DEL(rgbOdom);
	SYNC_DEL(rgbOdomScan2d);
	SYNC_DEL(rgbOdomScan3d);
	SYNC_DEL(rgbOdomInfo);
	SYNC_DEL(rgbOdomScan2dInfo);
	SYNC_DEL(rgbOdomScan3dInfo);

	// RGB-only + User Data
	SYNC_DEL(rgbData);
	SYNC_DEL(rgbDataScan2d);
	SYNC_DEL(rgbDataScan3d);
	SYNC_DEL(rgbDataInfo);
	SYNC_DEL(rgbDataScan2dInfo);
	SYNC_DEL(rgbDataScan3dInfo);

	// RGB-only + Odom + User Data
	SYNC_DEL(rgbOdomData);
	SYNC_DEL(rgbOdomDataScan2d);
	SYNC_DEL(rgbOdomDataScan3d);
	SYNC_DEL(rgbOdomDataInfo);
	SYNC_DEL(rgbOdomDataScan2dInfo);
	SYNC_DEL(rgbOdomDataScan3dInfo);

	// 1 RGBD
	SYNC_DEL(rgbdScan2d);
	SYNC_DEL(rgbdScan3d);
	SYNC_DEL(rgbdInfo);
	SYNC_DEL(rgbdScan2dInfo);
	SYNC_DEL(rgbdScan3dInfo);

	// 1 RGBD + Odom
	SYNC_DEL(rgbdOdom);
	SYNC_DEL(rgbdOdomScan2d);
	SYNC_DEL(rgbdOdomScan3d);
	SYNC_DEL(rgbdOdomInfo);
	SYNC_DEL(rgbdOdomScan2dInfo);
	SYNC_DEL(rgbdOdomScan3dInfo);

	// 1 RGBD + User Data
	SYNC_DEL(rgbdData);
	SYNC_DEL(rgbdDataScan2d);
	SYNC_DEL(rgbdDataScan3d);
	SYNC_DEL(rgbdDataInfo);
	SYNC_DEL(rgbdDataScan2dInfo);
	SYNC_DEL(rgbdDataScan3dInfo);

	// 1 RGBD + Odom + User Data
	SYNC_DEL(rgbdOdomData);
	SYNC_DEL(rgbdOdomDataScan2d);
	SYNC_DEL(rgbdOdomDataScan3d);
	SYNC_DEL(rgbdOdomDataInfo);
	SYNC_DEL(rgbdOdomDataScan2dInfo);
	SYNC_DEL(rgbdOdomDataScan3dInfo);

	// 2 RGBD
	SYNC_DEL(rgbd2);
	SYNC_DEL(rgbd2Scan2d);
	SYNC_DEL(rgbd2Scan3d);
	SYNC_DEL(rgbd2Info);
	SYNC_DEL(rgbd2Scan2dInfo);
	SYNC_DEL(rgbd2Scan3dInfo);

	// 2 RGBD + Odom
	SYNC_DEL(rgbd2Odom);
	SYNC_DEL(rgbd2OdomScan2d);
	SYNC_DEL(rgbd2OdomScan3d);
	SYNC_DEL(rgbd2OdomInfo);
	SYNC_DEL(rgbd2OdomScan2dInfo);
	SYNC_DEL(rgbd2OdomScan3dInfo);

	// 2 RGBD + User Data
	SYNC_DEL(rgbd2Data);
	SYNC_DEL(rgbd2DataScan2d);
	SYNC_DEL(rgbd2DataScan3d);
	SYNC_DEL(rgbd2DataInfo);
	SYNC_DEL(rgbd2DataScan2dInfo);
	SYNC_DEL(rgbd2DataScan3dInfo);

	// 2 RGBD + Odom + User Data
	SYNC_DEL(rgbd2OdomData);
	SYNC_DEL(rgbd2OdomDataScan2d);
	SYNC_DEL(rgbd2OdomDataScan3d);
	SYNC_DEL(rgbd2OdomDataInfo);
	SYNC_DEL(rgbd2OdomDataScan2dInfo);
	SYNC_DEL(rgbd2OdomDataScan3dInfo);

	// 3 RGBD
	SYNC_DEL(rgbd3);
	SYNC_DEL(rgbd3Scan2d);
	SYNC_DEL(rgbd3Scan3d);
	SYNC_DEL(rgbd3Info);
	SYNC_DEL(rgbd3Scan2dInfo);
	SYNC_DEL(rgbd3Scan3dInfo);

	// 3 RGBD + Odom
	SYNC_DEL(rgbd3Odom);
	SYNC_DEL(rgbd3OdomScan2d);
	SYNC_DEL(rgbd3OdomScan3d);
	SYNC_DEL(rgbd3OdomInfo);
	SYNC_DEL(rgbd3OdomScan2dInfo);
	SYNC_DEL(rgbd3OdomScan3dInfo);

	// 3 RGBD + User Data
	SYNC_DEL(rgbd3Data);
	SYNC_DEL(rgbd3DataScan2d);
	SYNC_DEL(rgbd3DataScan3d);
	SYNC_DEL(rgbd3DataInfo);
	SYNC_DEL(rgbd3DataScan2dInfo);
	SYNC_DEL(rgbd3DataScan3dInfo);

	// 3 RGBD + Odom + User Data
	SYNC_DEL(rgbd3OdomData);
	SYNC_DEL(rgbd3OdomDataScan2d);
	SYNC_DEL(rgbd3OdomDataScan3d);
	SYNC_DEL(rgbd3OdomDataInfo);
	SYNC_DEL(rgbd3OdomDataScan2dInfo);
	SYNC_DEL(rgbd3OdomDataScan3dInfo);

	// 4 RGBD
	SYNC_DEL(rgbd4);
	SYNC_DEL(rgbd4Scan2d);
	SYNC_DEL(rgbd4Scan3d);
	SYNC_DEL(rgbd4Info);
	SYNC_DEL(rgbd4Scan2dInfo);
	SYNC_DEL(rgbd4Scan3dInfo);

	// 4 RGBD + Odom
	SYNC_DEL(rgbd4Odom);
	SYNC_DEL(rgbd4OdomScan2d);
	SYNC_DEL(rgbd4OdomScan3d);
	SYNC_DEL(rgbd4OdomInfo);
	SYNC_DEL(rgbd4OdomScan2dInfo);
	SYNC_DEL(rgbd4OdomScan3dInfo);

	// 4 RGBD + User Data
	SYNC_DEL(rgbd4Data);
	SYNC_DEL(rgbd4DataScan2d);
	SYNC_DEL(rgbd4DataScan3d);
	SYNC_DEL(rgbd4DataInfo);
	SYNC_DEL(rgbd4DataScan2dInfo);
	SYNC_DEL(rgbd4DataScan3dInfo);

	// 4 RGBD + Odom + User Data
	SYNC_DEL(rgbd4OdomData);
	SYNC_DEL(rgbd4OdomDataScan2d);
	SYNC_DEL(rgbd4OdomDataScan3d);
	SYNC_DEL(rgbd4OdomDataInfo);
	SYNC_DEL(rgbd4OdomDataScan2dInfo);
	SYNC_DEL(rgbd4OdomDataScan3dInfo);

	// Scan
	SYNC_DEL(scan2dInfo);
	SYNC_DEL(scan3dInfo);

	// Scan + Odom
	SYNC_DEL(odomScan2d);
	SYNC_DEL(odomScan3d);
	SYNC_DEL(odomScan2dInfo);
	SYNC_DEL(odomScan3dInfo);

	// Scan + User Data
	SYNC_DEL(dataScan2d);
	SYNC_DEL(dataScan3d);
	SYNC_DEL(dataScan2dInfo);
	SYNC_DEL(dataScan3dInfo);

	// Scan + Odom + User Data
	SYNC_DEL(odomDataScan2d);
	SYNC_DEL(odomDataScan3d);
	SYNC_DEL(odomDataScan2dInfo);
	SYNC_DEL(odomDataScan3dInfo);

	// Odom
	SYNC_DEL(odomInfo);
	// Odom + User Data
	SYNC_DEL(odomData);
	SYNC_DEL(odomDataInfo);


	for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
	{
		delete rgbdSubs_[i];
	}
	rgbdSubs_.clear();

	//clear params
	ros::NodeHandle pnh("~");
	pnh.deleteParam("subscribe_depth");
	pnh.deleteParam("subscribe_laserScan");
	pnh.deleteParam("subscribe_scan");
	pnh.deleteParam("subscribe_scan_cloud");
	pnh.deleteParam("subscribe_stereo");
	pnh.deleteParam("subscribe_rgb");
	pnh.deleteParam("subscribe_rgbd");
	pnh.deleteParam("subscribe_odom_info");
	pnh.deleteParam("subscribe_user_data");
	pnh.deleteParam("odom_frame_id");
	pnh.deleteParam("rgbd_cameras");
	pnh.deleteParam("depth_cameras");
	pnh.deleteParam("queue_size");
	pnh.deleteParam("approx_sync");
	pnh.deleteParam("stereo_approx_sync");
}
CommonDataSubscriber::~CommonDataSubscriber()
{
	if(warningThread_)
	{
		callbackCalled();
		warningThread_->join();
		delete warningThread_;
	}

	// RGB + Depth
	SYNC_DEL(depth);
	SYNC_DEL(depthScan2d);
	SYNC_DEL(depthScan3d);
	SYNC_DEL(depthInfo);

	// RGB + Depth + Odom
	SYNC_DEL(depthOdom);
	SYNC_DEL(depthOdomScan2d);
	SYNC_DEL(depthOdomScan3d);
	SYNC_DEL(depthOdomInfo);

	// RGB + Depth + User Data
	SYNC_DEL(depthData);
	SYNC_DEL(depthDataScan2d);
	SYNC_DEL(depthDataScan3d);
	SYNC_DEL(depthDataInfo);

	// RGB + Depth + Odom + User Data
	SYNC_DEL(depthOdomData);
	SYNC_DEL(depthOdomDataScan2d);
	SYNC_DEL(depthOdomDataScan3d);
	SYNC_DEL(depthOdomDataInfo);

	// Stereo
	SYNC_DEL(stereo);
	SYNC_DEL(stereoInfo);

	// Stereo + Odom
	SYNC_DEL(stereoOdom);
	SYNC_DEL(stereoOdomInfo);

	// 1 RGBD
	SYNC_DEL(rgbdScan2d);
	SYNC_DEL(rgbdScan3d);
	SYNC_DEL(rgbdInfo);

	// 1 RGBD + Odom
	SYNC_DEL(rgbdOdom);
	SYNC_DEL(rgbdOdomScan2d);
	SYNC_DEL(rgbdOdomScan3d);
	SYNC_DEL(rgbdOdomInfo);

	// 1 RGBD + User Data
	SYNC_DEL(rgbdData);
	SYNC_DEL(rgbdDataScan2d);
	SYNC_DEL(rgbdDataScan3d);
	SYNC_DEL(rgbdDataInfo);

	// 1 RGBD + Odom + User Data
	SYNC_DEL(rgbdOdomData);
	SYNC_DEL(rgbdOdomDataScan2d);
	SYNC_DEL(rgbdOdomDataScan3d);
	SYNC_DEL(rgbdOdomDataInfo);

	// 2 RGBD
	SYNC_DEL(rgbd2);
	SYNC_DEL(rgbd2Scan2d);
	SYNC_DEL(rgbd2Scan3d);
	SYNC_DEL(rgbd2Info);

	// 2 RGBD + Odom
	SYNC_DEL(rgbd2Odom);
	SYNC_DEL(rgbd2OdomScan2d);
	SYNC_DEL(rgbd2OdomScan3d);
	SYNC_DEL(rgbd2OdomInfo);

	// 2 RGBD + User Data
	SYNC_DEL(rgbd2Data);
	SYNC_DEL(rgbd2DataScan2d);
	SYNC_DEL(rgbd2DataScan3d);
	SYNC_DEL(rgbd2DataInfo);

	// 2 RGBD + Odom + User Data
	SYNC_DEL(rgbd2OdomData);
	SYNC_DEL(rgbd2OdomDataScan2d);
	SYNC_DEL(rgbd2OdomDataScan3d);
	SYNC_DEL(rgbd2OdomDataInfo);

	for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
	{
		delete rgbdSubs_[i];
	}
	rgbdSubs_.clear();
}