if(rgbdCameras > 6)
			{
				ROS_ERROR("Cannot synchronize more than 6 rgbd topics (rgbd_cameras is set to %d). Set "
						"rgbd_cameras=0 to use RGBDImages interface instead, then "
						"synchronize RGBDImage topics yourself.", rgbdCameras);
			}

/*
All rights reserved.

*/

#include <rtabmap_ros/CommonDataSubscriber.h>

namespace rtabmap_ros {

CommonDataSubscriber::CommonDataSubscriber(bool gui) :
		queueSize_(10),
		approxSync_(true),
		warningThread_(0),
		callbackCalled_(false),
		subscribedToDepth_(!gui),
		subscribedToStereo_(false),
		subscribedToRGB_(!gui),
		subscribedToOdom_(false),
		subscribedToRGBD_(false),
		subscribedToScan2d_(false),
		subscribedToScan3d_(false),
		subscribedToScanDescriptor_(false),
		subscribedToOdomInfo_(false),

		// RGB + Depth
		SYNC_INIT(depth),
		SYNC_INIT(depthScan2d),
		SYNC_INIT(depthScan3d),
		SYNC_INIT(depthScanDesc),
		SYNC_INIT(depthInfo),
		SYNC_INIT(depthScan2dInfo),
		SYNC_INIT(depthScan3dInfo),
		SYNC_INIT(depthScanDescInfo),

		// RGB + Depth + Odom
		SYNC_INIT(depthOdom),
		SYNC_INIT(depthOdomScan2d),
		SYNC_INIT(depthOdomScan3d),
		SYNC_INIT(depthOdomScanDesc),
		SYNC_INIT(depthOdomInfo),
		SYNC_INIT(depthOdomScan2dInfo),
		SYNC_INIT(depthOdomScan3dInfo),
		SYNC_INIT(depthOdomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// RGB + Depth + User Data
		SYNC_INIT(depthData),
		SYNC_INIT(depthDataScan2d),
		SYNC_INIT(depthDataScan3d),
		SYNC_INIT(depthDataScanDesc),
		SYNC_INIT(depthDataInfo),
		SYNC_INIT(depthDataScan2dInfo),
		SYNC_INIT(depthDataScan3dInfo),
		SYNC_INIT(depthDataScanDescInfo),

		// RGB + Depth + Odom + User Data
		SYNC_INIT(depthOdomData),
		SYNC_INIT(depthOdomDataScan2d),
		SYNC_INIT(depthOdomDataScan3d),
		SYNC_INIT(depthOdomDataScanDesc),
		SYNC_INIT(depthOdomDataInfo),
		SYNC_INIT(depthOdomDataScan2dInfo),
		SYNC_INIT(depthOdomDataScan3dInfo),
		SYNC_INIT(depthOdomDataScanDescInfo),
#endif

		// Stereo
		SYNC_INIT(stereo),
		SYNC_INIT(stereoInfo),

		// Stereo + Odom
		SYNC_INIT(stereoOdom),
		SYNC_INIT(stereoOdomInfo),

		// RGB-only
		SYNC_INIT(rgb),
		SYNC_INIT(rgbScan2d),
		SYNC_INIT(rgbScan3d),
		SYNC_INIT(rgbScanDesc),
		SYNC_INIT(rgbInfo),
		SYNC_INIT(rgbScan2dInfo),
		SYNC_INIT(rgbScan3dInfo),
		SYNC_INIT(rgbScanDescInfo),

		// RGB-only + Odom
		SYNC_INIT(rgbOdom),
		SYNC_INIT(rgbOdomScan2d),
		SYNC_INIT(rgbOdomScan3d),
		SYNC_INIT(rgbOdomScanDesc),
		SYNC_INIT(rgbOdomInfo),
		SYNC_INIT(rgbOdomScan2dInfo),
		SYNC_INIT(rgbOdomScan3dInfo),
		SYNC_INIT(rgbOdomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// RGB-only + User Data
		SYNC_INIT(rgbData),
		SYNC_INIT(rgbDataScan2d),
		SYNC_INIT(rgbDataScan3d),
		SYNC_INIT(rgbDataScanDesc),
		SYNC_INIT(rgbDataInfo),
		SYNC_INIT(rgbDataScan2dInfo),
		SYNC_INIT(rgbDataScan3dInfo),
		SYNC_INIT(rgbDataScanDescInfo),

		// RGB-only + Odom + User Data
		SYNC_INIT(rgbOdomData),
		SYNC_INIT(rgbOdomDataScan2d),
		SYNC_INIT(rgbOdomDataScan3d),
		SYNC_INIT(rgbOdomDataScanDesc),
		SYNC_INIT(rgbOdomDataInfo),
		SYNC_INIT(rgbOdomDataScan2dInfo),
		SYNC_INIT(rgbOdomDataScan3dInfo),
		SYNC_INIT(rgbOdomDataScanDescInfo),
#endif
		SYNC_INIT(rgbdScan2d),
		SYNC_INIT(rgbdScan3d),
		SYNC_INIT(rgbdScanDesc),
		SYNC_INIT(rgbdInfo),

		SYNC_INIT(rgbdOdom),
		SYNC_INIT(rgbdOdomScan2d),
		SYNC_INIT(rgbdOdomScan3d),
		SYNC_INIT(rgbdOdomScanDesc),
		SYNC_INIT(rgbdOdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 1 RGBD + User Data
		SYNC_INIT(rgbdData),
		SYNC_INIT(rgbdDataScan2d),
		SYNC_INIT(rgbdDataScan3d),
		SYNC_INIT(rgbdDataScanDesc),
		SYNC_INIT(rgbdDataInfo),

		// 1 RGBD + Odom + User Data
		SYNC_INIT(rgbdOdomData),
		SYNC_INIT(rgbdOdomDataScan2d),
		SYNC_INIT(rgbdOdomDataScan3d),
		SYNC_INIT(rgbdOdomDataScanDesc),
		SYNC_INIT(rgbdOdomDataInfo),
#endif
		// X RGBD
		SYNC_INIT(rgbdXScan2d),
		SYNC_INIT(rgbdXScan3d),
		SYNC_INIT(rgbdXScanDesc),
		SYNC_INIT(rgbdXInfo),

		// X RGBD + Odom
		SYNC_INIT(rgbdXOdom),
		SYNC_INIT(rgbdXOdomScan2d),
		SYNC_INIT(rgbdXOdomScan3d),
		SYNC_INIT(rgbdXOdomScanDesc),
		SYNC_INIT(rgbdXOdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// X RGBD + User Data
		SYNC_INIT(rgbdXData),
		SYNC_INIT(rgbdXDataScan2d),
		SYNC_INIT(rgbdXDataScan3d),
		SYNC_INIT(rgbdXDataScanDesc),
		SYNC_INIT(rgbdXDataInfo),

		// X RGBD + Odom + User Data
		SYNC_INIT(rgbdXOdomData),
		SYNC_INIT(rgbdXOdomDataScan2d),
		SYNC_INIT(rgbdXOdomDataScan3d),
		SYNC_INIT(rgbdXOdomDataScanDesc),
		SYNC_INIT(rgbdXOdomDataInfo),
#endif

#ifdef RTABMAP_SYNC_MULTI_RGBD
		SYNC_INIT(rgbd2),
		SYNC_INIT(rgbd2Scan2d),
		SYNC_INIT(rgbd2Scan3d),
		SYNC_INIT(rgbd2ScanDesc),
		SYNC_INIT(rgbd2Info),

		SYNC_INIT(rgbd2Odom),
		SYNC_INIT(rgbd2OdomScan2d),
		SYNC_INIT(rgbd2OdomScan3d),
		SYNC_INIT(rgbd2OdomScanDesc),
		SYNC_INIT(rgbd2OdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 2 RGBD + User Data
		SYNC_INIT(rgbd2Data),
		SYNC_INIT(rgbd2DataScan2d),
		SYNC_INIT(rgbd2DataScan3d),
		SYNC_INIT(rgbd2DataScanDesc),
		SYNC_INIT(rgbd2DataInfo),

		// 2 RGBD + Odom + User Data
		SYNC_INIT(rgbd2OdomData),
		SYNC_INIT(rgbd2OdomDataScan2d),
		SYNC_INIT(rgbd2OdomDataScan3d),
		SYNC_INIT(rgbd2OdomDataScanDesc),
		SYNC_INIT(rgbd2OdomDataInfo),
#endif

		SYNC_INIT(rgbd3),
		SYNC_INIT(rgbd3Scan2d),
		SYNC_INIT(rgbd3Scan3d),
		SYNC_INIT(rgbd3ScanDesc),
		SYNC_INIT(rgbd3Info),

		SYNC_INIT(rgbd3Odom),
		SYNC_INIT(rgbd3OdomScan2d),
		SYNC_INIT(rgbd3OdomScan3d),
		SYNC_INIT(rgbd3OdomScanDesc),
		SYNC_INIT(rgbd3OdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 3 RGBD + User Data
		SYNC_INIT(rgbd3Data),
		SYNC_INIT(rgbd3DataScan2d),
		SYNC_INIT(rgbd3DataScan3d),
		SYNC_INIT(rgbd3DataScanDesc),
		SYNC_INIT(rgbd3DataInfo),

		// 3 RGBD + Odom + User Data
		SYNC_INIT(rgbd3OdomData),
		SYNC_INIT(rgbd3OdomDataScan2d),
		SYNC_INIT(rgbd3OdomDataScan3d),
		SYNC_INIT(rgbd3OdomDataScanDesc),
		SYNC_INIT(rgbd3OdomDataInfo),
#endif

		SYNC_INIT(rgbd4),
		SYNC_INIT(rgbd4Scan2d),
		SYNC_INIT(rgbd4Scan3d),
		SYNC_INIT(rgbd4ScanDesc),
		SYNC_INIT(rgbd4Info),

		SYNC_INIT(rgbd4Odom),
		SYNC_INIT(rgbd4OdomScan2d),
		SYNC_INIT(rgbd4OdomScan3d),
		SYNC_INIT(rgbd4OdomScanDesc),
		SYNC_INIT(rgbd4OdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 4 RGBD + User Data
		SYNC_INIT(rgbd4Data),
		SYNC_INIT(rgbd4DataScan2d),
		SYNC_INIT(rgbd4DataScan3d),
		SYNC_INIT(rgbd4DataScanDesc),
		SYNC_INIT(rgbd4DataInfo),

		// 4 RGBD + Odom + User Data
		SYNC_INIT(rgbd4OdomData),
		SYNC_INIT(rgbd4OdomDataScan2d),
		SYNC_INIT(rgbd4OdomDataScan3d),
		SYNC_INIT(rgbd4OdomDataScanDesc),
		SYNC_INIT(rgbd4OdomDataInfo),
#endif
		SYNC_INIT(rgbd5),
		SYNC_INIT(rgbd5Scan2d),
		SYNC_INIT(rgbd5Scan3d),
		SYNC_INIT(rgbd5ScanDesc),
		SYNC_INIT(rgbd5Info),

		SYNC_INIT(rgbd5Odom),
		SYNC_INIT(rgbd5OdomScan2d),
		SYNC_INIT(rgbd5OdomScan3d),
		SYNC_INIT(rgbd5OdomScanDesc),
		SYNC_INIT(rgbd5OdomInfo),

		SYNC_INIT(rgbd6),
		SYNC_INIT(rgbd6Scan2d),
		SYNC_INIT(rgbd6Scan3d),
		SYNC_INIT(rgbd6ScanDesc),
		SYNC_INIT(rgbd6Info),

		SYNC_INIT(rgbd6Odom),
		SYNC_INIT(rgbd6OdomScan2d),
		SYNC_INIT(rgbd6OdomScan3d),
		SYNC_INIT(rgbd6OdomScanDesc),
		SYNC_INIT(rgbd6OdomInfo),
#endif // RTABMAP_SYNC_MULTI_RGBD

		// Scan
		SYNC_INIT(scan2dInfo),
		SYNC_INIT(scan3dInfo),
		SYNC_INIT(scanDescInfo),

		// Scan + Odom
		SYNC_INIT(odomScan2d),
		SYNC_INIT(odomScan3d),
		SYNC_INIT(odomScanDesc),
		SYNC_INIT(odomScan2dInfo),
		SYNC_INIT(odomScan3dInfo),
		SYNC_INIT(odomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// Scan + User Data
		SYNC_INIT(dataScan2d),
		SYNC_INIT(dataScan3d),
		SYNC_INIT(dataScanDesc),
		SYNC_INIT(dataScan2dInfo),
		SYNC_INIT(dataScan3dInfo),
		SYNC_INIT(dataScanDescInfo),

		// Scan + Odom + User Data
		SYNC_INIT(odomDataScan2d),
		SYNC_INIT(odomDataScan3d),
		SYNC_INIT(odomDataScanDesc),
		SYNC_INIT(odomDataScan2dInfo),
		SYNC_INIT(odomDataScan3dInfo),
		SYNC_INIT(odomDataScanDescInfo),
#endif

		// Odom
		SYNC_INIT(odomInfo)
#ifdef RTABMAP_SYNC_USER_DATA
		,
		// Odom + User Data
		SYNC_INIT(odomData),
		SYNC_INIT(odomDataInfo)
#endif

{
}

void CommonDataSubscriber::setupCallbacks(
		ros::NodeHandle & nh,
		ros::NodeHandle & pnh,
		const std::string & name)
{
	bool subscribeScan2d = false;
	bool subscribeScan3d = false;
	bool subscribeScanDesc = false;
	bool subscribeOdomInfo = false;
	bool subscribeUserData = false;
	bool subscribeOdom = true;
	int rgbdCameras = 1;
	name_ = name;

	// ROS related parameters (private)
	pnh.param("subscribe_depth",     subscribedToDepth_, subscribedToDepth_);
	if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
	{
		ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
	}
	pnh.param("subscribe_rgb",       subscribedToRGB_, subscribedToRGB_);
	pnh.param("subscribe_scan",      subscribeScan2d, subscribeScan2d);
	pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
	pnh.param("subscribe_scan_descriptor", subscribeScanDesc, subscribeScanDesc);
	pnh.param("subscribe_stereo",    subscribedToStereo_, subscribedToStereo_);
	pnh.param("subscribe_rgbd",      subscribedToRGBD_, subscribedToRGBD_);
	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
	pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
	pnh.param("subscribe_odom",      subscribeOdom, subscribeOdom);

#ifdef RTABMAP_SYNC_USER_DATA
	if(subscribeUserData)
	{
		ROS_ERROR("subscribe_user_data is true, but rtabmap_ros has been built with RTABMAP_SYNC_USER_DATA. Setting back to false.");
		subscribeUserData = false;
	}
#endif

	if(subscribedToDepth_ && subscribedToStereo_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
		subscribedToDepth_ = false;
		subscribedToRGB_ = false;
	}
	if(subscribedToRGB_ && subscribedToStereo_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgb cannot be true at the same time. Parameter subscribe_rgb is set to false.");
		subscribedToRGB_ = false;
	}
	if(subscribedToDepth_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
		subscribedToDepth_ = false;
		subscribedToRGB_ = false;
	}
	if(subscribedToRGB_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
		subscribedToRGB_ = false;
	}
	if(subscribedToStereo_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
		subscribedToStereo_ = false;
	}
	if(subscribeScan2d && subscribeScan3d)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
		subscribeScan3d = false;
	}
	if(subscribeScan2d && subscribeScanDesc)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan is set to false.");
		subscribeScan2d = false;
	}
	if(subscribeScan3d && subscribeScanDesc)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan_cloud and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
		subscribeScan3d = false;
	}
	if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
	{
		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
		{
			approxSync_ = false; // default for scan: exact sync
		}
	}
	if(subscribedToStereo_)
	{
		approxSync_ = false; // default for stereo: exact sync
	}

	std::string odomFrameId;
	pnh.getParam("odom_frame_id", odomFrameId);
	pnh.param("rgbd_cameras",       rgbdCameras, rgbdCameras);
	if(pnh.hasParam("depth_cameras"))
	{
		ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
	}
	pnh.param("queue_size",          queueSize_, queueSize_);
	if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
	{
		ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
				 "to \"approx_sync\"! Your value is still copied to "
				 "corresponding parameter.");
		pnh.param("stereo_approx_sync", approxSync_, approxSync_);
	}
	else
	{
		pnh.param("approx_sync", approxSync_, approxSync_);
	}

	ROS_INFO("%s: subscribe_depth = %s", name.c_str(), subscribedToDepth_?"true":"false");
	ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
	ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
	ROS_INFO("%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras);
	ROS_INFO("%s: subscribe_odom_info = %s", name.c_str(), subscribeOdomInfo?"true":"false");
	ROS_INFO("%s: subscribe_user_data = %s", name.c_str(), subscribeUserData?"true":"false");
	ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false");
	ROS_INFO("%s: subscribe_scan_cloud = %s", name.c_str(), subscribeScan3d?"true":"false");
	ROS_INFO("%s: subscribe_scan_descriptor = %s", name.c_str(), subscribeScanDesc?"true":"false");
	ROS_INFO("%s: queue_size    = %d", name.c_str(), queueSize_);
	ROS_INFO("%s: approx_sync   = %s", name.c_str(), approxSync_?"true":"false");

	subscribedToOdom_ = odomFrameId.empty() && subscribeOdom;
	if(subscribedToDepth_)
	{
		setupDepthCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeUserData,
				subscribeScan2d,
				subscribeScan3d,
				subscribeScanDesc,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToStereo_)
	{
		setupStereoCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToRGB_)
	{
		setupRGBCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeUserData,
				subscribeScan2d,
				subscribeScan3d,
				subscribeScanDesc,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToRGBD_)
	{
		if(rgbdCameras == 0)
		{
			setupRGBDXCallbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
#ifdef RTABMAP_SYNC_MULTI_RGBD
		else 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,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
		else if(rgbdCameras == 3)
		{
			setupRGBD3Callbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
		else if(rgbdCameras == 2)
		{
			setupRGBD2Callbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
#else
		if(rgbdCameras>1)
		{
			ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has "
					"been built without RTABMAP_SYNC_MULTI_RGBD option). Set rgbd_cameras=0 to "
					"use RGBDImages interface instead without recompiling with RTABMAP_SYNC_MULTI_RGBD, "
					"but you will have to synchronize RGBDImage topics yourself.");
		}
#endif
		else
		{
			setupRGBDCallbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
	}
	else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
	{
		setupScanCallbacks(
					nh,
					pnh,
					subscribeScan2d,
					subscribeScanDesc,
					subscribedToOdom_,
					subscribeUserData,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
	}
	else if(subscribedToOdom_)
	{
		setupOdomCallbacks(
					nh,
					pnh,
					subscribeUserData,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
	}

	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
	{
		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
	}
}

CommonDataSubscriber::~CommonDataSubscriber()
{
	if(warningThread_)
	{
		callbackCalled();
		warningThread_->join();
		delete warningThread_;
	}

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

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

#ifdef RTABMAP_SYNC_USER_DATA
	// RGB + Depth + User Data
	SYNC_DEL(depthData);
	SYNC_DEL(depthDataScan2d);
	SYNC_DEL(depthDataScan3d);
	SYNC_DEL(depthDataScanDesc);
	SYNC_DEL(depthDataInfo);
	SYNC_DEL(depthDataScan2dInfo);
	SYNC_DEL(depthDataScan3dInfo);
	SYNC_DEL(depthDataScanDescInfo);

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

	// 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(rgbScanDesc);
	SYNC_DEL(rgbInfo);
	SYNC_DEL(rgbScan2dInfo);
	SYNC_DEL(rgbScan3dInfo);
	SYNC_DEL(rgbScanDescInfo);

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

#ifdef RTABMAP_SYNC_USER_DATA
	// RGB-only + User Data
	SYNC_DEL(rgbData);
	SYNC_DEL(rgbDataScan2d);
	SYNC_DEL(rgbDataScan3d);
	SYNC_DEL(rgbDataScanDesc);
	SYNC_DEL(rgbDataInfo);
	SYNC_DEL(rgbDataScan2dInfo);
	SYNC_DEL(rgbDataScan3dInfo);
	SYNC_DEL(rgbDataScanDescInfo);

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

	SYNC_DEL(rgbdScan2d);
	SYNC_DEL(rgbdScan3d);
	SYNC_DEL(rgbdScanDesc);
	SYNC_DEL(rgbdInfo);

	SYNC_DEL(rgbdOdom);
	SYNC_DEL(rgbdOdomScan2d);
	SYNC_DEL(rgbdOdomScan3d);
	SYNC_DEL(rgbdOdomScanDesc);
	SYNC_DEL(rgbdOdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 1 RGBD + User Data
	SYNC_DEL(rgbdData);
	SYNC_DEL(rgbdDataScan2d);
	SYNC_DEL(rgbdDataScan3d);
	SYNC_DEL(rgbdDataScanDesc);
	SYNC_DEL(rgbdDataInfo);

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

	// X RGBD
	SYNC_DEL(rgbdXScan2d);
	SYNC_DEL(rgbdXScan3d);
	SYNC_DEL(rgbdXScanDesc);
	SYNC_DEL(rgbdXInfo);

	// X RGBD + Odom
	SYNC_DEL(rgbdXOdom);
	SYNC_DEL(rgbdXOdomScan2d);
	SYNC_DEL(rgbdXOdomScan3d);
	SYNC_DEL(rgbdXOdomScanDesc);
	SYNC_DEL(rgbdXOdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// X RGBD + User Data
	SYNC_DEL(rgbdXData);
	SYNC_DEL(rgbdXDataScan2d);
	SYNC_DEL(rgbdXDataScan3d);
	SYNC_DEL(rgbdXDataScanDesc);
	SYNC_DEL(rgbdXDataInfo);

	// X RGBD + Odom + User Data
	SYNC_DEL(rgbdXOdomData);
	SYNC_DEL(rgbdXOdomDataScan2d);
	SYNC_DEL(rgbdXOdomDataScan3d);
	SYNC_DEL(rgbdXOdomDataScanDesc);
	SYNC_DEL(rgbdXOdomDataInfo);
#endif

#ifdef RTABMAP_SYNC_MULTI_RGBD
	SYNC_DEL(rgbd2);
	SYNC_DEL(rgbd2Scan2d);
	SYNC_DEL(rgbd2Scan3d);
	SYNC_DEL(rgbd2ScanDesc);
	SYNC_DEL(rgbd2Info);

	SYNC_DEL(rgbd2Odom);
	SYNC_DEL(rgbd2OdomScan2d);
	SYNC_DEL(rgbd2OdomScan3d);
	SYNC_DEL(rgbd2OdomScanDesc);
	SYNC_DEL(rgbd2OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 2 RGBD + User Data
	SYNC_DEL(rgbd2Data);
	SYNC_DEL(rgbd2DataScan2d);
	SYNC_DEL(rgbd2DataScan3d);
	SYNC_DEL(rgbd2DataScanDesc);
	SYNC_DEL(rgbd2DataInfo);

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

	SYNC_DEL(rgbd3);
	SYNC_DEL(rgbd3Scan2d);
	SYNC_DEL(rgbd3Scan3d);
	SYNC_DEL(rgbd3ScanDesc);
	SYNC_DEL(rgbd3Info);

	SYNC_DEL(rgbd3Odom);
	SYNC_DEL(rgbd3OdomScan2d);
	SYNC_DEL(rgbd3OdomScan3d);
	SYNC_DEL(rgbd3OdomScanDesc);
	SYNC_DEL(rgbd3OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 3 RGBD + User Data
	SYNC_DEL(rgbd3Data);
	SYNC_DEL(rgbd3DataScan2d);
	SYNC_DEL(rgbd3DataScan3d);
	SYNC_DEL(rgbd3DataScanDesc);
	SYNC_DEL(rgbd3DataInfo);

	// 3 RGBD + Odom + User Data
	SYNC_DEL(rgbd3OdomData);
	SYNC_DEL(rgbd3OdomDataScan2d);
	SYNC_DEL(rgbd3OdomDataScan3d);
	SYNC_DEL(rgbd3OdomDataScanDesc);
	SYNC_DEL(rgbd3OdomDataInfo);
#endif

	SYNC_DEL(rgbd4);
	SYNC_DEL(rgbd4Scan2d);
	SYNC_DEL(rgbd4Scan3d);
	SYNC_DEL(rgbd4ScanDesc);
	SYNC_DEL(rgbd4Info);

	SYNC_DEL(rgbd4Odom);
	SYNC_DEL(rgbd4OdomScan2d);
	SYNC_DEL(rgbd4OdomScan3d);
	SYNC_DEL(rgbd4OdomScanDesc);
	SYNC_DEL(rgbd4OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 4 RGBD + User Data
	SYNC_DEL(rgbd4Data);
	SYNC_DEL(rgbd4DataScan2d);
	SYNC_DEL(rgbd4DataScan3d);
	SYNC_DEL(rgbd4DataScanDesc);
	SYNC_DEL(rgbd4DataInfo);

	// 4 RGBD + Odom + User Data
	SYNC_DEL(rgbd4OdomData);
	SYNC_DEL(rgbd4OdomDataScan2d);
	SYNC_DEL(rgbd4OdomDataScan3d);
	SYNC_DEL(rgbd4OdomDataScanDesc);
	SYNC_DEL(rgbd4OdomDataInfo);
#endif
	SYNC_DEL(rgbd5);
	SYNC_DEL(rgbd5Scan2d);
	SYNC_DEL(rgbd5Scan3d);
	SYNC_DEL(rgbd5ScanDesc);
	SYNC_DEL(rgbd5Info);

	SYNC_DEL(rgbd5Odom);
	SYNC_DEL(rgbd5OdomScan2d);
	SYNC_DEL(rgbd5OdomScan3d);
	SYNC_DEL(rgbd5OdomScanDesc);
	SYNC_DEL(rgbd5OdomInfo);

	SYNC_DEL(rgbd6);
	SYNC_DEL(rgbd6Scan2d);
	SYNC_DEL(rgbd6Scan3d);
	SYNC_DEL(rgbd6ScanDesc);
	SYNC_DEL(rgbd6Info);

	SYNC_DEL(rgbd6Odom);
	SYNC_DEL(rgbd6OdomScan2d);
	SYNC_DEL(rgbd6OdomScan3d);
	SYNC_DEL(rgbd6OdomScanDesc);
	SYNC_DEL(rgbd6OdomInfo);
#endif //RTABMAP_SYNC_MULTI_RGBD

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

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

#ifdef RTABMAP_SYNC_USER_DATA
	// Scan + User Data
	SYNC_DEL(dataScan2d);
	SYNC_DEL(dataScan3d);
	SYNC_DEL(dataScanDesc);
	SYNC_DEL(dataScan2dInfo);
	SYNC_DEL(dataScan3dInfo);
	SYNC_DEL(dataScanDescInfo);

	// Scan + Odom + User Data
	SYNC_DEL(odomDataScan2d);
	SYNC_DEL(odomDataScan3d);
	SYNC_DEL(odomDataScanDesc);
	SYNC_DEL(odomDataScan2dInfo);
	SYNC_DEL(odomDataScan3dInfo);
	SYNC_DEL(odomDataScanDescInfo);
#endif

	// Odom
	SYNC_DEL(odomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
	// Odom + User Data
	SYNC_DEL(odomData);
	SYNC_DEL(odomDataInfo);
#endif

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

void CommonDataSubscriber::warningLoop()
{
	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. If topics are coming from different computers, make sure "
					"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
					name_.c_str(),
					approxSync_?
							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
							"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 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::LaserScan& scanMsg,
		const sensor_msgs::PointCloud2& scan3dMsg,
		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
		const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints,
		const std::vector<rtabmap_ros::Point3f> & localPoints3d,
		const cv::Mat & localDescriptors)
{
	callbackCalled();

	std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPointsMsgs;
	localKeyPointsMsgs.push_back(localKeyPoints);
	std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3dMsgs;
	localPoints3dMsgs.push_back(localPoints3d);
	std::vector<cv::Mat> localDescriptorsMsgs;
	localDescriptorsMsgs.push_back(localDescriptors);

	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,
				globalDescriptorMsgs,
				localKeyPointsMsgs,
				localPoints3dMsgs,
				localDescriptorsMsgs);
	}
	else // assuming stereo
	{
		commonStereoCallback(
				odomMsg,
				userDataMsg,
				imageMsg,
				depthMsg,
				rgbCameraInfoMsg,
				depthCameraInfoMsg,
				scanMsg,
				scan3dMsg,
				odomInfoMsg,
				globalDescriptorMsgs,
				localKeyPoints,
				localPoints3d,
				localDescriptors);
	}
}

} /* namespace rtabmap_ros */


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

/*
All rights reserved.

*/

#include <rtabmap_ros/CommonDataSubscriber.h>

namespace rtabmap_ros {

CommonDataSubscriber::CommonDataSubscriber(bool gui) :
		queueSize_(10),
		approxSync_(true),
		warningThread_(0),
		callbackCalled_(false),
		subscribedToDepth_(!gui),
		subscribedToStereo_(false),
		subscribedToRGB_(!gui),
		subscribedToOdom_(false),
		subscribedToRGBD_(false),
		subscribedToScan2d_(false),
		subscribedToScan3d_(false),
		subscribedToScanDescriptor_(false),
		subscribedToOdomInfo_(false),

		// RGB + Depth
		SYNC_INIT(depth),
		SYNC_INIT(depthScan2d),
		SYNC_INIT(depthScan3d),
		SYNC_INIT(depthScanDesc),
		SYNC_INIT(depthInfo),
		SYNC_INIT(depthScan2dInfo),
		SYNC_INIT(depthScan3dInfo),
		SYNC_INIT(depthScanDescInfo),

		// RGB + Depth + Odom
		SYNC_INIT(depthOdom),
		SYNC_INIT(depthOdomScan2d),
		SYNC_INIT(depthOdomScan3d),
		SYNC_INIT(depthOdomScanDesc),
		SYNC_INIT(depthOdomInfo),
		SYNC_INIT(depthOdomScan2dInfo),
		SYNC_INIT(depthOdomScan3dInfo),
		SYNC_INIT(depthOdomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// RGB + Depth + User Data
		SYNC_INIT(depthData),
		SYNC_INIT(depthDataScan2d),
		SYNC_INIT(depthDataScan3d),
		SYNC_INIT(depthDataScanDesc),
		SYNC_INIT(depthDataInfo),
		SYNC_INIT(depthDataScan2dInfo),
		SYNC_INIT(depthDataScan3dInfo),
		SYNC_INIT(depthDataScanDescInfo),

		// RGB + Depth + Odom + User Data
		SYNC_INIT(depthOdomData),
		SYNC_INIT(depthOdomDataScan2d),
		SYNC_INIT(depthOdomDataScan3d),
		SYNC_INIT(depthOdomDataScanDesc),
		SYNC_INIT(depthOdomDataInfo),
		SYNC_INIT(depthOdomDataScan2dInfo),
		SYNC_INIT(depthOdomDataScan3dInfo),
		SYNC_INIT(depthOdomDataScanDescInfo),
#endif

		// Stereo
		SYNC_INIT(stereo),
		SYNC_INIT(stereoInfo),

		// Stereo + Odom
		SYNC_INIT(stereoOdom),
		SYNC_INIT(stereoOdomInfo),

		// RGB-only
		SYNC_INIT(rgb),
		SYNC_INIT(rgbScan2d),
		SYNC_INIT(rgbScan3d),
		SYNC_INIT(rgbScanDesc),
		SYNC_INIT(rgbInfo),
		SYNC_INIT(rgbScan2dInfo),
		SYNC_INIT(rgbScan3dInfo),
		SYNC_INIT(rgbScanDescInfo),

		// RGB-only + Odom
		SYNC_INIT(rgbOdom),
		SYNC_INIT(rgbOdomScan2d),
		SYNC_INIT(rgbOdomScan3d),
		SYNC_INIT(rgbOdomScanDesc),
		SYNC_INIT(rgbOdomInfo),
		SYNC_INIT(rgbOdomScan2dInfo),
		SYNC_INIT(rgbOdomScan3dInfo),
		SYNC_INIT(rgbOdomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// RGB-only + User Data
		SYNC_INIT(rgbData),
		SYNC_INIT(rgbDataScan2d),
		SYNC_INIT(rgbDataScan3d),
		SYNC_INIT(rgbDataScanDesc),
		SYNC_INIT(rgbDataInfo),
		SYNC_INIT(rgbDataScan2dInfo),
		SYNC_INIT(rgbDataScan3dInfo),
		SYNC_INIT(rgbDataScanDescInfo),

		// RGB-only + Odom + User Data
		SYNC_INIT(rgbOdomData),
		SYNC_INIT(rgbOdomDataScan2d),
		SYNC_INIT(rgbOdomDataScan3d),
		SYNC_INIT(rgbOdomDataScanDesc),
		SYNC_INIT(rgbOdomDataInfo),
		SYNC_INIT(rgbOdomDataScan2dInfo),
		SYNC_INIT(rgbOdomDataScan3dInfo),
		SYNC_INIT(rgbOdomDataScanDescInfo),
#endif
		SYNC_INIT(rgbdScan2d),
		SYNC_INIT(rgbdScan3d),
		SYNC_INIT(rgbdScanDesc),
		SYNC_INIT(rgbdInfo),

		SYNC_INIT(rgbdOdom),
		SYNC_INIT(rgbdOdomScan2d),
		SYNC_INIT(rgbdOdomScan3d),
		SYNC_INIT(rgbdOdomScanDesc),
		SYNC_INIT(rgbdOdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 1 RGBD + User Data
		SYNC_INIT(rgbdData),
		SYNC_INIT(rgbdDataScan2d),
		SYNC_INIT(rgbdDataScan3d),
		SYNC_INIT(rgbdDataScanDesc),
		SYNC_INIT(rgbdDataInfo),

		// 1 RGBD + Odom + User Data
		SYNC_INIT(rgbdOdomData),
		SYNC_INIT(rgbdOdomDataScan2d),
		SYNC_INIT(rgbdOdomDataScan3d),
		SYNC_INIT(rgbdOdomDataScanDesc),
		SYNC_INIT(rgbdOdomDataInfo),
#endif
		// X RGBD
		SYNC_INIT(rgbdXScan2d),
		SYNC_INIT(rgbdXScan3d),
		SYNC_INIT(rgbdXScanDesc),
		SYNC_INIT(rgbdXInfo),

		// X RGBD + Odom
		SYNC_INIT(rgbdXOdom),
		SYNC_INIT(rgbdXOdomScan2d),
		SYNC_INIT(rgbdXOdomScan3d),
		SYNC_INIT(rgbdXOdomScanDesc),
		SYNC_INIT(rgbdXOdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// X RGBD + User Data
		SYNC_INIT(rgbdXData),
		SYNC_INIT(rgbdXDataScan2d),
		SYNC_INIT(rgbdXDataScan3d),
		SYNC_INIT(rgbdXDataScanDesc),
		SYNC_INIT(rgbdXDataInfo),

		// X RGBD + Odom + User Data
		SYNC_INIT(rgbdXOdomData),
		SYNC_INIT(rgbdXOdomDataScan2d),
		SYNC_INIT(rgbdXOdomDataScan3d),
		SYNC_INIT(rgbdXOdomDataScanDesc),
		SYNC_INIT(rgbdXOdomDataInfo),
#endif

#ifdef RTABMAP_SYNC_MULTI_RGBD
		SYNC_INIT(rgbd2),
		SYNC_INIT(rgbd2Scan2d),
		SYNC_INIT(rgbd2Scan3d),
		SYNC_INIT(rgbd2ScanDesc),
		SYNC_INIT(rgbd2Info),

		SYNC_INIT(rgbd2Odom),
		SYNC_INIT(rgbd2OdomScan2d),
		SYNC_INIT(rgbd2OdomScan3d),
		SYNC_INIT(rgbd2OdomScanDesc),
		SYNC_INIT(rgbd2OdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 2 RGBD + User Data
		SYNC_INIT(rgbd2Data),
		SYNC_INIT(rgbd2DataScan2d),
		SYNC_INIT(rgbd2DataScan3d),
		SYNC_INIT(rgbd2DataScanDesc),
		SYNC_INIT(rgbd2DataInfo),

		// 2 RGBD + Odom + User Data
		SYNC_INIT(rgbd2OdomData),
		SYNC_INIT(rgbd2OdomDataScan2d),
		SYNC_INIT(rgbd2OdomDataScan3d),
		SYNC_INIT(rgbd2OdomDataScanDesc),
		SYNC_INIT(rgbd2OdomDataInfo),
#endif

		SYNC_INIT(rgbd3),
		SYNC_INIT(rgbd3Scan2d),
		SYNC_INIT(rgbd3Scan3d),
		SYNC_INIT(rgbd3ScanDesc),
		SYNC_INIT(rgbd3Info),

		SYNC_INIT(rgbd3Odom),
		SYNC_INIT(rgbd3OdomScan2d),
		SYNC_INIT(rgbd3OdomScan3d),
		SYNC_INIT(rgbd3OdomScanDesc),
		SYNC_INIT(rgbd3OdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 3 RGBD + User Data
		SYNC_INIT(rgbd3Data),
		SYNC_INIT(rgbd3DataScan2d),
		SYNC_INIT(rgbd3DataScan3d),
		SYNC_INIT(rgbd3DataScanDesc),
		SYNC_INIT(rgbd3DataInfo),

		// 3 RGBD + Odom + User Data
		SYNC_INIT(rgbd3OdomData),
		SYNC_INIT(rgbd3OdomDataScan2d),
		SYNC_INIT(rgbd3OdomDataScan3d),
		SYNC_INIT(rgbd3OdomDataScanDesc),
		SYNC_INIT(rgbd3OdomDataInfo),
#endif

		SYNC_INIT(rgbd4),
		SYNC_INIT(rgbd4Scan2d),
		SYNC_INIT(rgbd4Scan3d),
		SYNC_INIT(rgbd4ScanDesc),
		SYNC_INIT(rgbd4Info),

		SYNC_INIT(rgbd4Odom),
		SYNC_INIT(rgbd4OdomScan2d),
		SYNC_INIT(rgbd4OdomScan3d),
		SYNC_INIT(rgbd4OdomScanDesc),
		SYNC_INIT(rgbd4OdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 4 RGBD + User Data
		SYNC_INIT(rgbd4Data),
		SYNC_INIT(rgbd4DataScan2d),
		SYNC_INIT(rgbd4DataScan3d),
		SYNC_INIT(rgbd4DataScanDesc),
		SYNC_INIT(rgbd4DataInfo),

		// 4 RGBD + Odom + User Data
		SYNC_INIT(rgbd4OdomData),
		SYNC_INIT(rgbd4OdomDataScan2d),
		SYNC_INIT(rgbd4OdomDataScan3d),
		SYNC_INIT(rgbd4OdomDataScanDesc),
		SYNC_INIT(rgbd4OdomDataInfo),
#endif
		SYNC_INIT(rgbd5),
		SYNC_INIT(rgbd5Scan2d),
		SYNC_INIT(rgbd5Scan3d),
		SYNC_INIT(rgbd5ScanDesc),
		SYNC_INIT(rgbd5Info),

		SYNC_INIT(rgbd5Odom),
		SYNC_INIT(rgbd5OdomScan2d),
		SYNC_INIT(rgbd5OdomScan3d),
		SYNC_INIT(rgbd5OdomScanDesc),
		SYNC_INIT(rgbd5OdomInfo),

		SYNC_INIT(rgbd6),
		SYNC_INIT(rgbd6Scan2d),
		SYNC_INIT(rgbd6Scan3d),
		SYNC_INIT(rgbd6ScanDesc),
		SYNC_INIT(rgbd6Info),

		SYNC_INIT(rgbd6Odom),
		SYNC_INIT(rgbd6OdomScan2d),
		SYNC_INIT(rgbd6OdomScan3d),
		SYNC_INIT(rgbd6OdomScanDesc),
		SYNC_INIT(rgbd6OdomInfo),
#endif // RTABMAP_SYNC_MULTI_RGBD

		// Scan
		SYNC_INIT(scan2dInfo),
		SYNC_INIT(scan3dInfo),
		SYNC_INIT(scanDescInfo),

		// Scan + Odom
		SYNC_INIT(odomScan2d),
		SYNC_INIT(odomScan3d),
		SYNC_INIT(odomScanDesc),
		SYNC_INIT(odomScan2dInfo),
		SYNC_INIT(odomScan3dInfo),
		SYNC_INIT(odomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// Scan + User Data
		SYNC_INIT(dataScan2d),
		SYNC_INIT(dataScan3d),
		SYNC_INIT(dataScanDesc),
		SYNC_INIT(dataScan2dInfo),
		SYNC_INIT(dataScan3dInfo),
		SYNC_INIT(dataScanDescInfo),

		// Scan + Odom + User Data
		SYNC_INIT(odomDataScan2d),
		SYNC_INIT(odomDataScan3d),
		SYNC_INIT(odomDataScanDesc),
		SYNC_INIT(odomDataScan2dInfo),
		SYNC_INIT(odomDataScan3dInfo),
		SYNC_INIT(odomDataScanDescInfo),
#endif

		// Odom
		SYNC_INIT(odomInfo)
#ifdef RTABMAP_SYNC_USER_DATA
		,
		// Odom + User Data
		SYNC_INIT(odomData),
		SYNC_INIT(odomDataInfo)
#endif

{
}

void CommonDataSubscriber::setupCallbacks(
		ros::NodeHandle & nh,
		ros::NodeHandle & pnh,
		const std::string & name)
{
	bool subscribeScan2d = false;
	bool subscribeScan3d = false;
	bool subscribeScanDesc = false;
	bool subscribeOdomInfo = false;
	bool subscribeUserData = false;
	bool subscribeOdom = true;
	int rgbdCameras = 1;
	name_ = name;

	// ROS related parameters (private)
	pnh.param("subscribe_depth",     subscribedToDepth_, subscribedToDepth_);
	if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
	{
		ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
	}
	pnh.param("subscribe_rgb",       subscribedToRGB_, subscribedToRGB_);
	pnh.param("subscribe_scan",      subscribeScan2d, subscribeScan2d);
	pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
	pnh.param("subscribe_scan_descriptor", subscribeScanDesc, subscribeScanDesc);
	pnh.param("subscribe_stereo",    subscribedToStereo_, subscribedToStereo_);
	pnh.param("subscribe_rgbd",      subscribedToRGBD_, subscribedToRGBD_);
	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
	pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
	pnh.param("subscribe_odom",      subscribeOdom, subscribeOdom);

#ifdef RTABMAP_SYNC_USER_DATA
	if(subscribeUserData)
	{
		ROS_ERROR("subscribe_user_data is true, but rtabmap_ros has been built with RTABMAP_SYNC_USER_DATA. Setting back to false.");
		subscribeUserData = false;
	}
#endif

	if(subscribedToDepth_ && subscribedToStereo_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
		subscribedToDepth_ = false;
		subscribedToRGB_ = false;
	}
	if(subscribedToRGB_ && subscribedToStereo_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgb cannot be true at the same time. Parameter subscribe_rgb is set to false.");
		subscribedToRGB_ = false;
	}
	if(subscribedToDepth_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
		subscribedToDepth_ = false;
		subscribedToRGB_ = false;
	}
	if(subscribedToRGB_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
		subscribedToRGB_ = false;
	}
	if(subscribedToStereo_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
		subscribedToStereo_ = false;
	}
	if(subscribeScan2d && subscribeScan3d)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
		subscribeScan3d = false;
	}
	if(subscribeScan2d && subscribeScanDesc)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan is set to false.");
		subscribeScan2d = false;
	}
	if(subscribeScan3d && subscribeScanDesc)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan_cloud and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
		subscribeScan3d = false;
	}
	if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
	{
		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
		{
			approxSync_ = false; // default for scan: exact sync
		}
	}
	if(subscribedToStereo_)
	{
		approxSync_ = false; // default for stereo: exact sync
	}

	std::string odomFrameId;
	pnh.getParam("odom_frame_id", odomFrameId);
	pnh.param("rgbd_cameras",       rgbdCameras, rgbdCameras);
	if(pnh.hasParam("depth_cameras"))
	{
		ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
	}
	pnh.param("queue_size",          queueSize_, queueSize_);
	if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
	{
		ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
				 "to \"approx_sync\"! Your value is still copied to "
				 "corresponding parameter.");
		pnh.param("stereo_approx_sync", approxSync_, approxSync_);
	}
	else
	{
		pnh.param("approx_sync", approxSync_, approxSync_);
	}

	ROS_INFO("%s: subscribe_depth = %s", name.c_str(), subscribedToDepth_?"true":"false");
	ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
	ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
	ROS_INFO("%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras);
	ROS_INFO("%s: subscribe_odom_info = %s", name.c_str(), subscribeOdomInfo?"true":"false");
	ROS_INFO("%s: subscribe_user_data = %s", name.c_str(), subscribeUserData?"true":"false");
	ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false");
	ROS_INFO("%s: subscribe_scan_cloud = %s", name.c_str(), subscribeScan3d?"true":"false");
	ROS_INFO("%s: subscribe_scan_descriptor = %s", name.c_str(), subscribeScanDesc?"true":"false");
	ROS_INFO("%s: queue_size    = %d", name.c_str(), queueSize_);
	ROS_INFO("%s: approx_sync   = %s", name.c_str(), approxSync_?"true":"false");

	subscribedToOdom_ = odomFrameId.empty() && subscribeOdom;
	if(subscribedToDepth_)
	{
		setupDepthCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeUserData,
				subscribeScan2d,
				subscribeScan3d,
				subscribeScanDesc,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToStereo_)
	{
		setupStereoCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToRGB_)
	{
		setupRGBCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeUserData,
				subscribeScan2d,
				subscribeScan3d,
				subscribeScanDesc,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToRGBD_)
	{
		if(rgbdCameras == 0)
		{
			setupRGBDXCallbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
#ifdef RTABMAP_SYNC_MULTI_RGBD
		else if(rgbdCameras >= 6)
		{
			if(rgbdCameras > 6)
			{
				ROS_ERROR("Cannot synchronize more than 6 rgbd topics (rgbd_cameras is set to %d). Set "
						"rgbd_cameras=0 to use RGBDImages interface instead, then "
						"synchronize RGBDImage topics yourself.", rgbdCameras);
			}

			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,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
		else if(rgbdCameras == 3)
		{
			setupRGBD3Callbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
		else if(rgbdCameras == 2)
		{
			setupRGBD2Callbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
#else
		if(rgbdCameras>1)
		{
			ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has "
					"been built without RTABMAP_SYNC_MULTI_RGBD option). Set rgbd_cameras=0 to "
					"use RGBDImages interface instead without recompiling with RTABMAP_SYNC_MULTI_RGBD, "
					"but you will have to synchronize RGBDImage topics yourself.");
		}
#endif
		else
		{
			setupRGBDCallbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
	}
	else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
	{
		setupScanCallbacks(
					nh,
					pnh,
					subscribeScan2d,
					subscribeScanDesc,
					subscribedToOdom_,
					subscribeUserData,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
	}
	else if(subscribedToOdom_)
	{
		setupOdomCallbacks(
					nh,
					pnh,
					subscribeUserData,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
	}

	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
	{
		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
	}
}

CommonDataSubscriber::~CommonDataSubscriber()
{
	if(warningThread_)
	{
		callbackCalled();
		warningThread_->join();
		delete warningThread_;
	}

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

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

#ifdef RTABMAP_SYNC_USER_DATA
	// RGB + Depth + User Data
	SYNC_DEL(depthData);
	SYNC_DEL(depthDataScan2d);
	SYNC_DEL(depthDataScan3d);
	SYNC_DEL(depthDataScanDesc);
	SYNC_DEL(depthDataInfo);
	SYNC_DEL(depthDataScan2dInfo);
	SYNC_DEL(depthDataScan3dInfo);
	SYNC_DEL(depthDataScanDescInfo);

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

	// 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(rgbScanDesc);
	SYNC_DEL(rgbInfo);
	SYNC_DEL(rgbScan2dInfo);
	SYNC_DEL(rgbScan3dInfo);
	SYNC_DEL(rgbScanDescInfo);

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

#ifdef RTABMAP_SYNC_USER_DATA
	// RGB-only + User Data
	SYNC_DEL(rgbData);
	SYNC_DEL(rgbDataScan2d);
	SYNC_DEL(rgbDataScan3d);
	SYNC_DEL(rgbDataScanDesc);
	SYNC_DEL(rgbDataInfo);
	SYNC_DEL(rgbDataScan2dInfo);
	SYNC_DEL(rgbDataScan3dInfo);
	SYNC_DEL(rgbDataScanDescInfo);

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

	SYNC_DEL(rgbdScan2d);
	SYNC_DEL(rgbdScan3d);
	SYNC_DEL(rgbdScanDesc);
	SYNC_DEL(rgbdInfo);

	SYNC_DEL(rgbdOdom);
	SYNC_DEL(rgbdOdomScan2d);
	SYNC_DEL(rgbdOdomScan3d);
	SYNC_DEL(rgbdOdomScanDesc);
	SYNC_DEL(rgbdOdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 1 RGBD + User Data
	SYNC_DEL(rgbdData);
	SYNC_DEL(rgbdDataScan2d);
	SYNC_DEL(rgbdDataScan3d);
	SYNC_DEL(rgbdDataScanDesc);
	SYNC_DEL(rgbdDataInfo);

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

	// X RGBD
	SYNC_DEL(rgbdXScan2d);
	SYNC_DEL(rgbdXScan3d);
	SYNC_DEL(rgbdXScanDesc);
	SYNC_DEL(rgbdXInfo);

	// X RGBD + Odom
	SYNC_DEL(rgbdXOdom);
	SYNC_DEL(rgbdXOdomScan2d);
	SYNC_DEL(rgbdXOdomScan3d);
	SYNC_DEL(rgbdXOdomScanDesc);
	SYNC_DEL(rgbdXOdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// X RGBD + User Data
	SYNC_DEL(rgbdXData);
	SYNC_DEL(rgbdXDataScan2d);
	SYNC_DEL(rgbdXDataScan3d);
	SYNC_DEL(rgbdXDataScanDesc);
	SYNC_DEL(rgbdXDataInfo);

	// X RGBD + Odom + User Data
	SYNC_DEL(rgbdXOdomData);
	SYNC_DEL(rgbdXOdomDataScan2d);
	SYNC_DEL(rgbdXOdomDataScan3d);
	SYNC_DEL(rgbdXOdomDataScanDesc);
	SYNC_DEL(rgbdXOdomDataInfo);
#endif

#ifdef RTABMAP_SYNC_MULTI_RGBD
	SYNC_DEL(rgbd2);
	SYNC_DEL(rgbd2Scan2d);
	SYNC_DEL(rgbd2Scan3d);
	SYNC_DEL(rgbd2ScanDesc);
	SYNC_DEL(rgbd2Info);

	SYNC_DEL(rgbd2Odom);
	SYNC_DEL(rgbd2OdomScan2d);
	SYNC_DEL(rgbd2OdomScan3d);
	SYNC_DEL(rgbd2OdomScanDesc);
	SYNC_DEL(rgbd2OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 2 RGBD + User Data
	SYNC_DEL(rgbd2Data);
	SYNC_DEL(rgbd2DataScan2d);
	SYNC_DEL(rgbd2DataScan3d);
	SYNC_DEL(rgbd2DataScanDesc);
	SYNC_DEL(rgbd2DataInfo);

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

	SYNC_DEL(rgbd3);
	SYNC_DEL(rgbd3Scan2d);
	SYNC_DEL(rgbd3Scan3d);
	SYNC_DEL(rgbd3ScanDesc);
	SYNC_DEL(rgbd3Info);

	SYNC_DEL(rgbd3Odom);
	SYNC_DEL(rgbd3OdomScan2d);
	SYNC_DEL(rgbd3OdomScan3d);
	SYNC_DEL(rgbd3OdomScanDesc);
	SYNC_DEL(rgbd3OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 3 RGBD + User Data
	SYNC_DEL(rgbd3Data);
	SYNC_DEL(rgbd3DataScan2d);
	SYNC_DEL(rgbd3DataScan3d);
	SYNC_DEL(rgbd3DataScanDesc);
	SYNC_DEL(rgbd3DataInfo);

	// 3 RGBD + Odom + User Data
	SYNC_DEL(rgbd3OdomData);
	SYNC_DEL(rgbd3OdomDataScan2d);
	SYNC_DEL(rgbd3OdomDataScan3d);
	SYNC_DEL(rgbd3OdomDataScanDesc);
	SYNC_DEL(rgbd3OdomDataInfo);
#endif

	SYNC_DEL(rgbd4);
	SYNC_DEL(rgbd4Scan2d);
	SYNC_DEL(rgbd4Scan3d);
	SYNC_DEL(rgbd4ScanDesc);
	SYNC_DEL(rgbd4Info);

	SYNC_DEL(rgbd4Odom);
	SYNC_DEL(rgbd4OdomScan2d);
	SYNC_DEL(rgbd4OdomScan3d);
	SYNC_DEL(rgbd4OdomScanDesc);
	SYNC_DEL(rgbd4OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 4 RGBD + User Data
	SYNC_DEL(rgbd4Data);
	SYNC_DEL(rgbd4DataScan2d);
	SYNC_DEL(rgbd4DataScan3d);
	SYNC_DEL(rgbd4DataScanDesc);
	SYNC_DEL(rgbd4DataInfo);

	// 4 RGBD + Odom + User Data
	SYNC_DEL(rgbd4OdomData);
	SYNC_DEL(rgbd4OdomDataScan2d);
	SYNC_DEL(rgbd4OdomDataScan3d);
	SYNC_DEL(rgbd4OdomDataScanDesc);
	SYNC_DEL(rgbd4OdomDataInfo);
#endif
	SYNC_DEL(rgbd5);
	SYNC_DEL(rgbd5Scan2d);
	SYNC_DEL(rgbd5Scan3d);
	SYNC_DEL(rgbd5ScanDesc);
	SYNC_DEL(rgbd5Info);

	SYNC_DEL(rgbd5Odom);
	SYNC_DEL(rgbd5OdomScan2d);
	SYNC_DEL(rgbd5OdomScan3d);
	SYNC_DEL(rgbd5OdomScanDesc);
	SYNC_DEL(rgbd5OdomInfo);

	SYNC_DEL(rgbd6);
	SYNC_DEL(rgbd6Scan2d);
	SYNC_DEL(rgbd6Scan3d);
	SYNC_DEL(rgbd6ScanDesc);
	SYNC_DEL(rgbd6Info);

	SYNC_DEL(rgbd6Odom);
	SYNC_DEL(rgbd6OdomScan2d);
	SYNC_DEL(rgbd6OdomScan3d);
	SYNC_DEL(rgbd6OdomScanDesc);
	SYNC_DEL(rgbd6OdomInfo);
#endif //RTABMAP_SYNC_MULTI_RGBD

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

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

#ifdef RTABMAP_SYNC_USER_DATA
	// Scan + User Data
	SYNC_DEL(dataScan2d);
	SYNC_DEL(dataScan3d);
	SYNC_DEL(dataScanDesc);
	SYNC_DEL(dataScan2dInfo);
	SYNC_DEL(dataScan3dInfo);
	SYNC_DEL(dataScanDescInfo);

	// Scan + Odom + User Data
	SYNC_DEL(odomDataScan2d);
	SYNC_DEL(odomDataScan3d);
	SYNC_DEL(odomDataScanDesc);
	SYNC_DEL(odomDataScan2dInfo);
	SYNC_DEL(odomDataScan3dInfo);
	SYNC_DEL(odomDataScanDescInfo);
#endif

	// Odom
	SYNC_DEL(odomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
	// Odom + User Data
	SYNC_DEL(odomData);
	SYNC_DEL(odomDataInfo);
#endif

	rgbdSubs_.clear();
}

void CommonDataSubscriber::warningLoop()
{
	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. If topics are coming from different computers, make sure "
					"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
					name_.c_str(),
					approxSync_?
							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
							"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 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::LaserScan& scanMsg,
		const sensor_msgs::PointCloud2& scan3dMsg,
		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
		const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints,
		const std::vector<rtabmap_ros::Point3f> & localPoints3d,
		const cv::Mat & localDescriptors)
{
	callbackCalled();

	std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPointsMsgs;
	localKeyPointsMsgs.push_back(localKeyPoints);
	std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3dMsgs;
	localPoints3dMsgs.push_back(localPoints3d);
	std::vector<cv::Mat> localDescriptorsMsgs;
	localDescriptorsMsgs.push_back(localDescriptors);

	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,
				globalDescriptorMsgs,
				localKeyPointsMsgs,
				localPoints3dMsgs,
				localDescriptorsMsgs);
	}
	else // assuming stereo
	{
		commonStereoCallback(
				odomMsg,
				userDataMsg,
				imageMsg,
				depthMsg,
				rgbCameraInfoMsg,
				depthCameraInfoMsg,
				scanMsg,
				scan3dMsg,
				odomInfoMsg,
				globalDescriptorMsgs,
				localKeyPoints,
				localPoints3d,
				localDescriptors);
	}
}

} /* namespace rtabmap_ros */


/*
All rights reserved.

*/

#include <rtabmap_ros/CommonDataSubscriber.h>

namespace rtabmap_ros {

CommonDataSubscriber::CommonDataSubscriber(bool gui) :
		queueSize_(10),
		approxSync_(true),
		warningThread_(0),
		callbackCalled_(false),
		subscribedToDepth_(!gui),
		subscribedToStereo_(false),
		subscribedToRGB_(!gui),
		subscribedToOdom_(false),
		subscribedToRGBD_(false),
		subscribedToScan2d_(false),
		subscribedToScan3d_(false),
		subscribedToScanDescriptor_(false),
		subscribedToOdomInfo_(false),

		// RGB + Depth
		SYNC_INIT(depth),
		SYNC_INIT(depthScan2d),
		SYNC_INIT(depthScan3d),
		SYNC_INIT(depthScanDesc),
		SYNC_INIT(depthInfo),
		SYNC_INIT(depthScan2dInfo),
		SYNC_INIT(depthScan3dInfo),
		SYNC_INIT(depthScanDescInfo),

		// RGB + Depth + Odom
		SYNC_INIT(depthOdom),
		SYNC_INIT(depthOdomScan2d),
		SYNC_INIT(depthOdomScan3d),
		SYNC_INIT(depthOdomScanDesc),
		SYNC_INIT(depthOdomInfo),
		SYNC_INIT(depthOdomScan2dInfo),
		SYNC_INIT(depthOdomScan3dInfo),
		SYNC_INIT(depthOdomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// RGB + Depth + User Data
		SYNC_INIT(depthData),
		SYNC_INIT(depthDataScan2d),
		SYNC_INIT(depthDataScan3d),
		SYNC_INIT(depthDataScanDesc),
		SYNC_INIT(depthDataInfo),
		SYNC_INIT(depthDataScan2dInfo),
		SYNC_INIT(depthDataScan3dInfo),
		SYNC_INIT(depthDataScanDescInfo),

		// RGB + Depth + Odom + User Data
		SYNC_INIT(depthOdomData),
		SYNC_INIT(depthOdomDataScan2d),
		SYNC_INIT(depthOdomDataScan3d),
		SYNC_INIT(depthOdomDataScanDesc),
		SYNC_INIT(depthOdomDataInfo),
		SYNC_INIT(depthOdomDataScan2dInfo),
		SYNC_INIT(depthOdomDataScan3dInfo),
		SYNC_INIT(depthOdomDataScanDescInfo),
#endif

		// Stereo
		SYNC_INIT(stereo),
		SYNC_INIT(stereoInfo),

		// Stereo + Odom
		SYNC_INIT(stereoOdom),
		SYNC_INIT(stereoOdomInfo),

		// RGB-only
		SYNC_INIT(rgb),
		SYNC_INIT(rgbScan2d),
		SYNC_INIT(rgbScan3d),
		SYNC_INIT(rgbScanDesc),
		SYNC_INIT(rgbInfo),
		SYNC_INIT(rgbScan2dInfo),
		SYNC_INIT(rgbScan3dInfo),
		SYNC_INIT(rgbScanDescInfo),

		// RGB-only + Odom
		SYNC_INIT(rgbOdom),
		SYNC_INIT(rgbOdomScan2d),
		SYNC_INIT(rgbOdomScan3d),
		SYNC_INIT(rgbOdomScanDesc),
		SYNC_INIT(rgbOdomInfo),
		SYNC_INIT(rgbOdomScan2dInfo),
		SYNC_INIT(rgbOdomScan3dInfo),
		SYNC_INIT(rgbOdomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// RGB-only + User Data
		SYNC_INIT(rgbData),
		SYNC_INIT(rgbDataScan2d),
		SYNC_INIT(rgbDataScan3d),
		SYNC_INIT(rgbDataScanDesc),
		SYNC_INIT(rgbDataInfo),
		SYNC_INIT(rgbDataScan2dInfo),
		SYNC_INIT(rgbDataScan3dInfo),
		SYNC_INIT(rgbDataScanDescInfo),

		// RGB-only + Odom + User Data
		SYNC_INIT(rgbOdomData),
		SYNC_INIT(rgbOdomDataScan2d),
		SYNC_INIT(rgbOdomDataScan3d),
		SYNC_INIT(rgbOdomDataScanDesc),
		SYNC_INIT(rgbOdomDataInfo),
		SYNC_INIT(rgbOdomDataScan2dInfo),
		SYNC_INIT(rgbOdomDataScan3dInfo),
		SYNC_INIT(rgbOdomDataScanDescInfo),
#endif
		SYNC_INIT(rgbdScan2d),
		SYNC_INIT(rgbdScan3d),
		SYNC_INIT(rgbdScanDesc),
		SYNC_INIT(rgbdInfo),

		SYNC_INIT(rgbdOdom),
		SYNC_INIT(rgbdOdomScan2d),
		SYNC_INIT(rgbdOdomScan3d),
		SYNC_INIT(rgbdOdomScanDesc),
		SYNC_INIT(rgbdOdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 1 RGBD + User Data
		SYNC_INIT(rgbdData),
		SYNC_INIT(rgbdDataScan2d),
		SYNC_INIT(rgbdDataScan3d),
		SYNC_INIT(rgbdDataScanDesc),
		SYNC_INIT(rgbdDataInfo),

		// 1 RGBD + Odom + User Data
		SYNC_INIT(rgbdOdomData),
		SYNC_INIT(rgbdOdomDataScan2d),
		SYNC_INIT(rgbdOdomDataScan3d),
		SYNC_INIT(rgbdOdomDataScanDesc),
		SYNC_INIT(rgbdOdomDataInfo),
#endif
		// X RGBD
		SYNC_INIT(rgbdXScan2d),
		SYNC_INIT(rgbdXScan3d),
		SYNC_INIT(rgbdXScanDesc),
		SYNC_INIT(rgbdXInfo),

		// X RGBD + Odom
		SYNC_INIT(rgbdXOdom),
		SYNC_INIT(rgbdXOdomScan2d),
		SYNC_INIT(rgbdXOdomScan3d),
		SYNC_INIT(rgbdXOdomScanDesc),
		SYNC_INIT(rgbdXOdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// X RGBD + User Data
		SYNC_INIT(rgbdXData),
		SYNC_INIT(rgbdXDataScan2d),
		SYNC_INIT(rgbdXDataScan3d),
		SYNC_INIT(rgbdXDataScanDesc),
		SYNC_INIT(rgbdXDataInfo),

		// X RGBD + Odom + User Data
		SYNC_INIT(rgbdXOdomData),
		SYNC_INIT(rgbdXOdomDataScan2d),
		SYNC_INIT(rgbdXOdomDataScan3d),
		SYNC_INIT(rgbdXOdomDataScanDesc),
		SYNC_INIT(rgbdXOdomDataInfo),
#endif

#ifdef RTABMAP_SYNC_MULTI_RGBD
		SYNC_INIT(rgbd2),
		SYNC_INIT(rgbd2Scan2d),
		SYNC_INIT(rgbd2Scan3d),
		SYNC_INIT(rgbd2ScanDesc),
		SYNC_INIT(rgbd2Info),

		SYNC_INIT(rgbd2Odom),
		SYNC_INIT(rgbd2OdomScan2d),
		SYNC_INIT(rgbd2OdomScan3d),
		SYNC_INIT(rgbd2OdomScanDesc),
		SYNC_INIT(rgbd2OdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 2 RGBD + User Data
		SYNC_INIT(rgbd2Data),
		SYNC_INIT(rgbd2DataScan2d),
		SYNC_INIT(rgbd2DataScan3d),
		SYNC_INIT(rgbd2DataScanDesc),
		SYNC_INIT(rgbd2DataInfo),

		// 2 RGBD + Odom + User Data
		SYNC_INIT(rgbd2OdomData),
		SYNC_INIT(rgbd2OdomDataScan2d),
		SYNC_INIT(rgbd2OdomDataScan3d),
		SYNC_INIT(rgbd2OdomDataScanDesc),
		SYNC_INIT(rgbd2OdomDataInfo),
#endif

		SYNC_INIT(rgbd3),
		SYNC_INIT(rgbd3Scan2d),
		SYNC_INIT(rgbd3Scan3d),
		SYNC_INIT(rgbd3ScanDesc),
		SYNC_INIT(rgbd3Info),

		SYNC_INIT(rgbd3Odom),
		SYNC_INIT(rgbd3OdomScan2d),
		SYNC_INIT(rgbd3OdomScan3d),
		SYNC_INIT(rgbd3OdomScanDesc),
		SYNC_INIT(rgbd3OdomInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// 3 RGBD + User Data
		SYNC_INIT(rgbd3Data),
		SYNC_INIT(rgbd3DataScan2d),
		SYNC_INIT(rgbd3DataScan3d),
		SYNC_INIT(rgbd3DataScanDesc),
		SYNC_INIT(rgbd3DataInfo),

		// 3 RGBD + Odom + User Data
		SYNC_INIT(rgbd3OdomData),
		SYNC_INIT(rgbd3OdomDataScan2d),
		SYNC_INIT(rgbd3OdomDataScan3d),
		SYNC_INIT(rgbd3OdomDataScanDesc),
		SYNC_INIT(rgbd3OdomDataInfo),
#endif

		SYNC_INIT(rgbd4),
		SYNC_INIT(rgbd4Scan2d),
		SYNC_INIT(rgbd4Scan3d),
		SYNC_INIT(rgbd4ScanDesc),
		SYNC_INIT(rgbd4Info),

		SYNC_INIT(rgbd4Odom),
		SYNC_INIT(rgbd4OdomScan2d),
		SYNC_INIT(rgbd4OdomScan3d),
		SYNC_INIT(rgbd4OdomScanDesc),
		SYNC_INIT(rgbd4OdomInfo),

#ifdef RT

ABMAP_SYNC_USER_DATA
		// 4 RGBD + User Data
		SYNC_INIT(rgbd4Data),
		SYNC_INIT(rgbd4DataScan2d),
		SYNC_INIT(rgbd4DataScan3d),
		SYNC_INIT(rgbd4DataScanDesc),
		SYNC_INIT(rgbd4DataInfo),

		// 4 RGBD + Odom + User Data
		SYNC_INIT(rgbd4OdomData),
		SYNC_INIT(rgbd4OdomDataScan2d),
		SYNC_INIT(rgbd4OdomDataScan3d),
		SYNC_INIT(rgbd4OdomDataScanDesc),
		SYNC_INIT(rgbd4OdomDataInfo),
#endif
		SYNC_INIT(rgbd5),
		SYNC_INIT(rgbd5Scan2d),
		SYNC_INIT(rgbd5Scan3d),
		SYNC_INIT(rgbd5ScanDesc),
		SYNC_INIT(rgbd5Info),

		SYNC_INIT(rgbd5Odom),
		SYNC_INIT(rgbd5OdomScan2d),
		SYNC_INIT(rgbd5OdomScan3d),
		SYNC_INIT(rgbd5OdomScanDesc),
		SYNC_INIT(rgbd5OdomInfo),

		SYNC_INIT(rgbd6),
		SYNC_INIT(rgbd6Scan2d),
		SYNC_INIT(rgbd6Scan3d),
		SYNC_INIT(rgbd6ScanDesc),
		SYNC_INIT(rgbd6Info),

		SYNC_INIT(rgbd6Odom),
		SYNC_INIT(rgbd6OdomScan2d),
		SYNC_INIT(rgbd6OdomScan3d),
		SYNC_INIT(rgbd6OdomScanDesc),
		SYNC_INIT(rgbd6OdomInfo),
#endif // RTABMAP_SYNC_MULTI_RGBD

		// Scan
		SYNC_INIT(scan2dInfo),
		SYNC_INIT(scan3dInfo),
		SYNC_INIT(scanDescInfo),

		// Scan + Odom
		SYNC_INIT(odomScan2d),
		SYNC_INIT(odomScan3d),
		SYNC_INIT(odomScanDesc),
		SYNC_INIT(odomScan2dInfo),
		SYNC_INIT(odomScan3dInfo),
		SYNC_INIT(odomScanDescInfo),

#ifdef RTABMAP_SYNC_USER_DATA
		// Scan + User Data
		SYNC_INIT(dataScan2d),
		SYNC_INIT(dataScan3d),
		SYNC_INIT(dataScanDesc),
		SYNC_INIT(dataScan2dInfo),
		SYNC_INIT(dataScan3dInfo),
		SYNC_INIT(dataScanDescInfo),

		// Scan + Odom + User Data
		SYNC_INIT(odomDataScan2d),
		SYNC_INIT(odomDataScan3d),
		SYNC_INIT(odomDataScanDesc),
		SYNC_INIT(odomDataScan2dInfo),
		SYNC_INIT(odomDataScan3dInfo),
		SYNC_INIT(odomDataScanDescInfo),
#endif

		// Odom
		SYNC_INIT(odomInfo)
#ifdef RTABMAP_SYNC_USER_DATA
		,
		// Odom + User Data
		SYNC_INIT(odomData),
		SYNC_INIT(odomDataInfo)
#endif

{
}

void CommonDataSubscriber::setupCallbacks(
		ros::NodeHandle & nh,
		ros::NodeHandle & pnh,
		const std::string & name)
{
	bool subscribeScan2d = false;
	bool subscribeScan3d = false;
	bool subscribeScanDesc = false;
	bool subscribeOdomInfo = false;
	bool subscribeUserData = false;
	bool subscribeOdom = true;
	int rgbdCameras = 1;
	name_ = name;

	// ROS related parameters (private)
	pnh.param("subscribe_depth",     subscribedToDepth_, subscribedToDepth_);
	if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
	{
		ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
	}
	pnh.param("subscribe_rgb",       subscribedToRGB_, subscribedToRGB_);
	pnh.param("subscribe_scan",      subscribeScan2d, subscribeScan2d);
	pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
	pnh.param("subscribe_scan_descriptor", subscribeScanDesc, subscribeScanDesc);
	pnh.param("subscribe_stereo",    subscribedToStereo_, subscribedToStereo_);
	pnh.param("subscribe_rgbd",      subscribedToRGBD_, subscribedToRGBD_);
	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
	pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
	pnh.param("subscribe_odom",      subscribeOdom, subscribeOdom);

#ifdef RTABMAP_SYNC_USER_DATA
	if(subscribeUserData)
	{
		ROS_ERROR("subscribe_user_data is true, but rtabmap_ros has been built with RTABMAP_SYNC_USER_DATA. Setting back to false.");
		subscribeUserData = false;
	}
#endif

	if(subscribedToDepth_ && subscribedToStereo_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
		subscribedToDepth_ = false;
		subscribedToRGB_ = false;
	}
	if(subscribedToRGB_ && subscribedToStereo_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgb cannot be true at the same time. Parameter subscribe_rgb is set to false.");
		subscribedToRGB_ = false;
	}
	if(subscribedToDepth_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
		subscribedToDepth_ = false;
		subscribedToRGB_ = false;
	}
	if(subscribedToRGB_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
		subscribedToRGB_ = false;
	}
	if(subscribedToStereo_ && subscribedToRGBD_)
	{
		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
		subscribedToStereo_ = false;
	}
	if(subscribeScan2d && subscribeScan3d)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
		subscribeScan3d = false;
	}
	if(subscribeScan2d && subscribeScanDesc)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan is set to false.");
		subscribeScan2d = false;
	}
	if(subscribeScan3d && subscribeScanDesc)
	{
		ROS_WARN("rtabmap: Parameters subscribe_scan_cloud and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
		subscribeScan3d = false;
	}
	if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
	{
		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
		{
			approxSync_ = false; // default for scan: exact sync
		}
	}
	if(subscribedToStereo_)
	{
		approxSync_ = false; // default for stereo: exact sync
	}

	std::string odomFrameId;
	pnh.getParam("odom_frame_id", odomFrameId);
	pnh.param("rgbd_cameras",       rgbdCameras, rgbdCameras);
	if(pnh.hasParam("depth_cameras"))
	{
		ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
	}
	pnh.param("queue_size",          queueSize_, queueSize_);
	if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
	{
		ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
				 "to \"approx_sync\"! Your value is still copied to "
				 "corresponding parameter.");
		pnh.param("stereo_approx_sync", approxSync_, approxSync_);
	}
	else
	{
		pnh.param("approx_sync", approxSync_, approxSync_);
	}

	ROS_INFO("%s: subscribe_depth = %s", name.c_str(), subscribedToDepth_?"true":"false");
	ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
	ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
	ROS_INFO("%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras);
	ROS_INFO("%s: subscribe_odom_info = %s", name.c_str(), subscribeOdomInfo?"true":"false");
	ROS_INFO("%s: subscribe_user_data = %s", name.c_str(), subscribeUserData?"true":"false");
	ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false");
	ROS_INFO("%s: subscribe_scan_cloud = %s", name.c_str(), subscribeScan3d?"true":"false");
	ROS_INFO("%s: subscribe_scan_descriptor = %s", name.c_str(), subscribeScanDesc?"true":"false");
	ROS_INFO("%s: queue_size    = %d", name.c_str(), queueSize_);
	ROS_INFO("%s: approx_sync   = %s", name.c_str(), approxSync_?"true":"false");

	subscribedToOdom_ = odomFrameId.empty() && subscribeOdom;
	if(subscribedToDepth_)
	{
		setupDepthCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeUserData,
				subscribeScan2d,
				subscribeScan3d,
				subscribeScanDesc,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToStereo_)
	{
		setupStereoCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToRGB_)
	{
		setupRGBCallbacks(
				nh,
				pnh,
				subscribedToOdom_,
				subscribeUserData,
				subscribeScan2d,
				subscribeScan3d,
				subscribeScanDesc,
				subscribeOdomInfo,
				queueSize_,
				approxSync_);
	}
	else if(subscribedToRGBD_)
	{
		if(rgbdCameras == 0)
		{
			setupRGBDXCallbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
#ifdef RTABMAP_SYNC_MULTI_RGBD
		else if(rgbdCameras >= 6)
		{
			if(rgbdCameras > 6)
			{
				ROS_ERROR("Cannot synchronize more than 6 rgbd topics (rgbd_cameras is set to %d). Set "
						"rgbd_cameras=0 to use RGBDImages interface instead, then "
						"synchronize RGBDImage topics yourself.", rgbdCameras);
			}

			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,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
		else if(rgbdCameras == 3)
		{
			setupRGBD3Callbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
		else if(rgbdCameras == 2)
		{
			setupRGBD2Callbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
#else
		if(rgbdCameras>1)
		{
			ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has "
					"been built without RTABMAP_SYNC_MULTI_RGBD option). Set rgbd_cameras=0 to "
					"use RGBDImages interface instead without recompiling with RTABMAP_SYNC_MULTI_RGBD, "
					"but you will have to synchronize RGBDImage topics yourself.");
		}
#endif
		else
		{
			setupRGBDCallbacks(
					nh,
					pnh,
					subscribedToOdom_,
					subscribeUserData,
					subscribeScan2d,
					subscribeScan3d,
					subscribeScanDesc,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
		}
	}
	else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
	{
		setupScanCallbacks(
					nh,
					pnh,
					subscribeScan2d,
					subscribeScanDesc,
					subscribedToOdom_,
					subscribeUserData,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
	}
	else if(subscribedToOdom_)
	{
		setupOdomCallbacks(
					nh,
					pnh,
					subscribeUserData,
					subscribeOdomInfo,
					queueSize_,
					approxSync_);
	}

	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
	{
		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
	}
}

CommonDataSubscriber::~CommonDataSubscriber()
{
	if(warningThread_)
	{
		callbackCalled();
		warningThread_->join();
		delete warningThread_;
	}

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

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

#ifdef RTABMAP_SYNC_USER_DATA
	// RGB + Depth + User Data
	SYNC_DEL(depthData);
	SYNC_DEL(depthDataScan2d);
	SYNC_DEL(depthDataScan3d);
	SYNC_DEL(depthDataScanDesc);
	SYNC_DEL(depthDataInfo);
	SYNC_DEL(depthDataScan2dInfo);
	SYNC_DEL(depthDataScan3dInfo);
	SYNC_DEL(depthDataScanDescInfo);

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

	// 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(rgbScanDesc);
	SYNC_DEL(rgbInfo);
	SYNC_DEL(rgbScan2dInfo);
	SYNC_DEL(rgbScan3dInfo);
	SYNC_DEL(rgbScanDescInfo);

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

#ifdef RTABMAP_SYNC_USER_DATA
	// RGB-only + User Data
	SYNC_DEL(rgbData);
	SYNC_DEL(rgbDataScan2d);
	SYNC_DEL(rgbDataScan3d);
	SYNC_DEL(rgbDataScanDesc);
	SYNC_DEL(rgbDataInfo);
	SYNC_DEL(rgbDataScan2dInfo);
	SYNC_DEL(rgbDataScan3dInfo);
	SYNC_DEL(rgbDataScanDescInfo);

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

	SYNC_DEL(rgbdScan2d);
	SYNC_DEL(rgbdScan3d);
	SYNC_DEL(rgbdScanDesc);
	SYNC_DEL(rgbdInfo);

	SYNC_DEL(rgbdOdom);
	SYNC_DEL(rgbdOdomScan2d);
	SYNC_DEL(rgbdOdomScan3d);
	SYNC_DEL(rgbdOdomScanDesc);
	SYNC_DEL(rgbdOdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 1 RGBD + User Data
	SYNC_DEL(rgbdData);
	SYNC_DEL(rgbdDataScan2d);
	SYNC_DEL(rgbdDataScan3d);
	SYNC_DEL(rgbdDataScanDesc);
	SYNC_DEL(rgbdDataInfo);

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

	// X RGBD
	SYNC_DEL(rgbdXScan2d);
	SYNC_DEL(rgbdXScan3d);
	SYNC_DEL(rgbdXScanDesc);
	SYNC_DEL(rgbdXInfo);

	// X RGBD + Odom
	SYNC_DEL(rgbdXOdom);
	SYNC_DEL(rgbdXOdomScan2d);
	SYNC_DEL(rgbdXOdomScan3d);
	SYNC_DEL(rgbdXOdomScanDesc);
	SYNC_DEL(rgbdXOdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// X RGBD + User Data
	SYNC_DEL(rgbdXData);
	SYNC_DEL(rgbdXDataScan2d);
	SYNC_DEL(rgbdXDataScan3d);
	SYNC_DEL(rgbdXDataScanDesc);
	SYNC_DEL(rgbdXDataInfo);

	// X RGBD + Odom + User Data
	SYNC_DEL(rgbdXOdomData);
	SYNC_DEL(rgbdXOdomDataScan2d);
	SYNC_DEL(rgbdXOdomDataScan3d);
	SYNC_DEL(rgbdXOdomDataScanDesc);
	SYNC_DEL(rgbdXOdomDataInfo);
#endif

#ifdef RTABMAP_SYNC_MULTI_RGBD
	SYNC_DEL(rgbd2);
	SYNC_DEL(rgbd2Scan2d);
	SYNC_DEL(rgbd2Scan3d);
	SYNC_DEL(rgbd2ScanDesc);
	SYNC_DEL(rgbd2Info);

	SYNC_DEL(rgbd2Odom);
	SYNC_DEL(rgbd2OdomScan2d);
	SYNC_DEL(rgbd2OdomScan3d);
	SYNC_DEL(rgbd2OdomScanDesc);
	SYNC_DEL(rgbd2OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 2 RGBD + User Data
	SYNC_DEL(rgbd2Data);
	SYNC_DEL(rgbd2DataScan2d);
	SYNC_DEL(rgbd2DataScan3d);
	SYNC_DEL(rgbd2DataScanDesc);
	SYNC_DEL(rgbd2DataInfo);

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

	SYNC_DEL(rgbd3);
	SYNC_DEL(rgbd3Scan2d);
	SYNC_DEL(rgbd3Scan3d);
	SYNC_DEL(rgbd3ScanDesc);
	SYNC_DEL(rgbd3Info);

	SYNC_DEL(rgbd3Odom);
	SYNC_DEL(rgbd3OdomScan2d);
	SYNC_DEL(rgbd3OdomScan3d);
	SYNC_DEL(rgbd3OdomScanDesc);
	SYNC_DEL(rgbd3OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 3 RGBD + User Data
	SYNC_DEL(rgbd3Data);
	SYNC_DEL(rgbd3DataScan2d);
	SYNC_DEL(rgbd3DataScan3d);
	SYNC_DEL(rgbd3DataScanDesc);
	SYNC_DEL(rgbd3DataInfo);

	// 3 RGBD + Odom + User Data
	SYNC_DEL(rgbd3OdomData);
	SYNC_DEL(rgbd3OdomDataScan2d);
	SYNC_DEL(rgbd3OdomDataScan3d);
	SYNC_DEL(rgbd3OdomDataScanDesc);
	SYNC_DEL(rgbd3OdomDataInfo);
#endif

	SYNC_DEL(rgbd4);
	SYNC_DEL(rgbd4Scan2d);
	SYNC_DEL(rgbd4Scan3d);
	SYNC_DEL(rgbd4ScanDesc);
	SYNC_DEL(rgbd4Info);

	SYNC_DEL(rgbd4Odom);
	SYNC_DEL(rgbd4OdomScan2d);
	SYNC_DEL(rgbd4OdomScan3d);
	SYNC_DEL(rgbd4OdomScanDesc);
	SYNC_DEL(rgbd4OdomInfo);

#ifdef RTABMAP_SYNC_USER_DATA
	// 4 RGBD + User Data
	SYNC_DEL(rgbd4Data);
	SYNC_DEL(rgbd4DataScan2d);
	SYNC_DEL(rgbd4DataScan3d);
	SYNC_DEL(rgbd4DataScanDesc);
	SYNC_DEL(rgbd4DataInfo);

	// 4 RGBD + Odom + User Data
	SYNC_DEL(rgbd4OdomData);
	SYNC_DEL(rgbd4OdomDataScan2d);
	SYNC_DEL(rgbd4OdomDataScan3d);
	SYNC_DEL(rgbd4OdomDataScanDesc);
	SYNC_DEL(rgbd4OdomDataInfo);
#endif
	SYNC_DEL(rgbd5);
	SYNC_DEL(rgbd5Scan2d);
	SYNC_DEL(rgbd5Scan3d);
	SYNC_DEL(rgbd5ScanDesc);
	SYNC_DEL(rgbd5Info);

	SYNC_DEL(rgbd5Odom);
	SYNC_DEL(rgbd5OdomScan2d);
	SYNC_DEL(rgbd5OdomScan3d);
	SYNC_DEL(rgbd5OdomScanDesc);
	SYNC_DEL(rgbd5OdomInfo);

	SYNC_DEL(rgbd6);
	SYNC_DEL(rgbd6Scan2d);
	SYNC_DEL(rgbd6Scan3d);
	SYNC_DEL(rgbd6ScanDesc);
	SYNC_DEL(rgbd6Info);

	SYNC_DEL(rgbd6Odom);
	SYNC_DEL(rgbd6OdomScan2d);
	SYNC_DEL(rgbd6OdomScan3d);
	SYNC_DEL(rgbd6OdomScanDesc);
	SYNC_DEL(rgbd6OdomInfo);
#endif //RTABMAP_SYNC_MULTI_RGBD

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

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

#ifdef RTABMAP_SYNC_USER_DATA
	// Scan + User Data
	SYNC_DEL(dataScan2d);
	SYNC_DEL(dataScan3d);
	SYNC_DEL(dataScanDesc);
	SYNC_DEL(dataScan2dInfo);
	SYNC_DEL(dataScan3dInfo);
	SYNC_DEL(dataScanDescInfo);

	// Scan + Odom + User Data
	SYNC_DEL(odomDataScan2d);
	SYNC_DEL(odomDataScan3d);
	SYNC_DEL(odomDataScanDesc);
	SYNC_DEL(odomDataScan2dInfo);
	SYNC_DEL(odomDataScan3dInfo);
	SYNC_DEL(odomDataScanDescInfo);
#endif

	// Odom
	SYNC_DEL(odomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
	// Odom + User Data
	SYNC_DEL(odomData);
	SYNC_DEL(odomDataInfo);
#endif

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

void CommonDataSubscriber::warningLoop()
{
	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. If topics are coming from different computers, make sure "
					"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
					name_.c_str(),
					approxSync_?
							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
							"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 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::LaserScan& scanMsg,
		const sensor_msgs::PointCloud2& scan3dMsg,
		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
		const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints,
		const std::vector<rtabmap_ros::Point3f> & localPoints3d,
		const cv::Mat & localDescriptors)
{
	callbackCalled();

	std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPointsMsgs;
	localKeyPointsMsgs.push_back(localKeyPoints);
	std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3dMsgs;
	localPoints3dMsgs.push_back(localPoints3d);
	std::vector<cv::Mat> localDescriptorsMsgs;
	localDescriptorsMsgs.push_back(localDescriptors);

	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,
				globalDescriptorMsgs,
				localKeyPointsMsgs,
				localPoints3dMsgs,
				localDescriptorsMsgs);
	}
	else // assuming stereo
	{
		commonStereoCallback(
				odomMsg,
				userDataMsg,
				imageMsg,
				depthMsg,
				rgbCameraInfoMsg,
				depthCameraInfoMsg,
				scanMsg,
				scan3dMsg,
				odomInfoMsg,
				globalDescriptorMsgs,
				localKeyPoints,
				localPoints3d,
				localDescriptors);
	}
}

} /* namespace rtabmap_ros */


#include "Catch.hpp"

#include "RaZ/Math/MathUtils.hpp"
#include "RaZ/Math/Vector.hpp"

TEST_CASE("MathUtils lerp arithmetic types") {
  CHECK(Raz::MathUtils::lerp(0.f, 1.f, 0.f) == 0.f);
  CHECK(Raz::MathUtils::lerp(0.f, 1.f, 0.25f) == 0.25f);
  CHECK(Raz::MathUtils::lerp(0.f, 1.f, 0.5f) == 0.5f);
  CHECK(Raz::MathUtils::lerp(0.f, 1.f, 0.75f) == 0.75f);
  CHECK(Raz::MathUtils::lerp(0.f, 1.f, 1.f) == 1.f);

  CHECK(Raz::MathUtils::lerp(-5.f, 5.f, 0.f) == -5.f);
  CHECK(Raz::MathUtils::lerp(-5.f, 5.f, 0.25f) == -2.5f);
  CHECK(Raz::MathUtils::lerp(-5.f, 5.f, 0.5f) == 0.f);
  CHECK(Raz::MathUtils::lerp(-5.f, 5.f, 0.75f) == 2.5f);
  CHECK(Raz::MathUtils::lerp(-5.f, 5.f, 1.f) == 5.f);

  CHECK(Raz::MathUtils::lerp(-1.43f, 8.12f, 0.f) == -1.43f);
  CHECK(Raz::MathUtils::lerp(-1.43f, 8.12f, 0.25f) == 0.9575f);
  CHECK(Raz::MathUtils::lerp(-1.43f, 8.12f, 0.5f) == 3.345f);
  CHECK(Raz::MathUtils::lerp(-1.43f, 8.12f, 0.75f) == 5.7325001f);
  CHECK(Raz::MathUtils::lerp(-1.43f, 8.12f, 1.f) == 8.12f);

  CHECK(Raz::MathUtils::lerp(457, 5324, 0.f) == 457);
  CHECK(Raz::MathUtils::lerp(457, 5324, 0.25f) == 1673);
  CHECK(Raz::MathUtils::lerp(457, 5324, 0.5f) == 2890);
  CHECK(Raz::MathUtils::lerp(457, 5324, 0.75f) == 4107);
  CHECK(Raz::MathUtils::lerp(457, 5324, 1.f) == 5324);
}

TEST_CASE("MathUtils lerp vector") {
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(0.f), Raz::Vec3f(1.f), 0.f) == Raz::Vec3f(0.f));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(0.f), Raz::Vec3f(1.f), 0.25f) == Raz::Vec3f(0.25f));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(0.f), Raz::Vec3f(1.f), 0.5f) == Raz::Vec3f(0.5f));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(0.f), Raz::Vec3f(1.f), 0.75f) == Raz::Vec3f(0.75f));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(0.f), Raz::Vec3f(1.f), 1.f) == Raz::Vec3f(1.f));

  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(1.f, 2.f, 3.f), Raz::Vec3f(3.f, 2.f, 1.f), 0.f) == Raz::Vec3f(1.f, 2.f, 3.f));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(1.f, 2.f, 3.f), Raz::Vec3f(3.f, 2.f, 1.f), 0.25f) == Raz::Vec3f(1.5f, 2.f, 2.5f));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(1.f, 2.f, 3.f), Raz::Vec3f(3.f, 2.f, 1.f), 0.5f) == Raz::Vec3f(2.f, 2.f, 2.f));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(1.f, 2.f, 3.f), Raz::Vec3f(3.f, 2.f, 1.f), 0.75f) == Raz::Vec3f(2.5f, 2.f, 1.5f));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3f(1.f, 2.f, 3.f), Raz::Vec3f(3.f, 2.f, 1.f), 1.f) == Raz::Vec3f(3.f, 2.f, 1.f));

  CHECK(Raz::MathUtils::lerp(Raz::Vec3i(140, 874, 3541), Raz::Vec3i(841, 53, 4712), 0.f) == Raz::Vec3i(140, 874, 3541));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3i(140, 874, 3541), Raz::Vec3i(841, 53, 4712), 0.25f) == Raz::Vec3i(315, 668, 3833));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3i(140, 874, 3541), Raz::Vec3i(841, 53, 4712), 0.5f) == Raz::Vec3i(490, 463, 4126));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3i(140, 874, 3541), Raz::Vec3i(841, 53, 4712), 0.75f) == Raz::Vec3i(665, 258, 4419));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3i(140, 874, 3541), Raz::Vec3i(841, 53, 4712), 1.f) == Raz::Vec3i(841, 53, 4712));

  CHECK(Raz::MathUtils::lerp(Raz::Vec3b(255, 140, 27), Raz::Vec3b(75, 241, 173), 0.f) == Raz::Vec3b(255, 140, 27));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3b(255, 140, 27), Raz::Vec3b(75, 241, 173), 0.25f) == Raz::Vec3b(210, 165, 63));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3b(255, 140, 27), Raz::Vec3b(75, 241, 173), 0.5f) == Raz::Vec3b(165, 190, 100));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3b(255, 140, 27), Raz::Vec3b(75, 241, 173), 0.75f) == Raz::Vec3b(120, 215, 136));
  CHECK(Raz::MathUtils::lerp(Raz::Vec3b(255, 140, 27), Raz::Vec3b(75, 241, 173), 1.f) == Raz::Vec3b(75, 241, 173));
}

TEST_CASE("MathUtils smoothstep value") {
  CHECK(Raz::MathUtils::smoothstep(0.f) == 0.f);
  CHECK(Raz::MathUtils::smoothstep(0.25f) == 0.15625f);
  CHECK(Raz::MathUtils::smoothstep(0.5f) == 0.5f);
  CHECK(Raz::MathUtils::smoothstep(0.75f) == 0.84375f);
  CHECK(Raz::MathUtils::smoothstep(1.f) == 1.f);
}

TEST_CASE("MathUtils smoothstep range") {
  CHECK(Raz::MathUtils::smoothstep(0.f, 1.f, -1.f) == 0.f); // If value < minThresh, returns 0
  CHECK(Raz::MathUtils::smoothstep(0.f, 1.f, 0.f) == 0.f);
  CHECK(Raz::MathUtils::smoothstep(0.f, 1.f, 0.25f) == 0.15625f);
  CHECK(Raz::MathUtils::smoothstep(0.f, 1.f, 0.5f) == 0.5f);
  CHECK(Raz::MathUtils::smoothstep(0.f, 1.f, 0.75f) == 0.84375f);
  CHECK(Raz::MathUtils::smoothstep(0.f, 1.f, 1.f) == 1.f);
  CHECK(Raz::MathUtils::smoothstep(0.f, 1.f, 2.f) == 1.f); // If value > maxThresh, returns 1

  CHEC

K(Raz::MathUtils::smoothstep(-5.f, 5.f, -10.f) == 0.f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, -5.f - std::numeric_limits<float>::epsilon()) == 0.f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, -5.f) == 0.f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, -4.5f) == 0.00725000072f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, -2.5f) == 0.15625f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, -0.5f) == 0.42524996f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, 0.f) == 0.5f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, 0.5f) == 0.57475001f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, 2.5f) == 0.84375f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, 4.5f) == 0.99274999f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, 5.f) == 1.f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, 5.f + std::numeric_limits<float>::epsilon()) == 1.f);
  CHECK(Raz::MathUtils::smoothstep(-5.f, 5.f, 10.f) == 1.f);
}

TEST_CASE("MathUtils smootherstep value") {
  CHECK(Raz::MathUtils::smootherstep(0.f) == 0.f);
  CHECK(Raz::MathUtils::smootherstep(0.25f) == 0.103515625f);
  CHECK(Raz::MathUtils::smootherstep(0.5f) == 0.5f);
  CHECK(Raz::MathUtils::smootherstep(0.75f) == 0.89648438f);
  CHECK(Raz::MathUtils::smootherstep(1.f) == 1.f);
}

TEST_CASE("MathUtils smootherstep range") {
  CHECK(Raz::MathUtils::smootherstep(0.f, 1.f, -1.f) == 0.f); // If value < minThresh, returns 0
  CHECK(Raz::MathUtils::smootherstep(0.f, 1.f, 0.f) == 0.f);
  CHECK(Raz::MathUtils::smootherstep(0.f, 1.f, 0.25f) == 0.103515625f);
  CHECK(Raz::MathUtils::smootherstep(0.f, 1.f, 0.5f) == 0.5f);
  CHECK(Raz::MathUtils::smootherstep(0.f, 1.f, 0.75f) == 0.89648438f);
  CHECK(Raz::MathUtils::smootherstep(0.f, 1.f, 1.f) == 1.f);
  CHECK(Raz::MathUtils::smootherstep(0.f, 1.f, 2.f) == 1.f); // If value > maxThresh, returns 1

  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, -10.f) == 0.f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, -5.f - std::numeric_limits<float>::epsilon()) == 0.f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, -5.f) == 0.f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, -4.5f) == 0.0011581251f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, -2.5f) == 0.103515625f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, -0.5f) == 0.40687308f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, 0.f) == 0.5f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, 0.5f) == 0.59312695f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, 2.5f) == 0.89648438f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, 4.5f) == 0.99884182f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, 5.f) == 1.f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, 5.f + std::numeric_limits<float>::epsilon()) == 1.f);
  CHECK(Raz::MathUtils::smootherstep(-5.f, 5.f, 10.f) == 1.f);
}


void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)

/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)
{

}

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;

  double cps_diff (0.0);

  // TODO this implementation rotates the control points, look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

  ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

  for (int j = 1; j < ncp + 1; j++)
  {
    m_solver.K (row, 2 * ((j + 0) % ncp) + 0, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 0, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 0, 1.0 * weight);
    row++;

    m_solver.K (row, 2 * ((j + 0) % ncp) + 1, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 1, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 1, 1.0 * weight);
    row++;
  }
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}