CommonSubscribers: added RGB-only callbacks, added odom-only callbacks. Odom: Fixed reset_odom service assert.

src/CommonDataSubscriber.cpp

@@ -36,6 +36,8 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		callbackCalled_(false),
 		subscribedToDepth_(!gui),
 		subscribedToStereo_(false),
+		subscribedToRGB_(false),
+		subscribedToOdom_(false),
 		subscribedToRGBD_(false),
 		subscribedToScan2d_(false),
 		subscribedToScan3d_(false),
@@ -81,6 +83,38 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(stereoOdom),
 		SYNC_INIT(stereoOdomInfo),
 
+		// RGB-only
+		SYNC_INIT(rgb),
+		SYNC_INIT(rgbScan2d),
+		SYNC_INIT(rgbScan3d),
+		SYNC_INIT(rgbInfo),
+		SYNC_INIT(rgbScan2dInfo),
+		SYNC_INIT(rgbScan3dInfo),
+
+		// RGB-only + Odom
+		SYNC_INIT(rgbOdom),
+		SYNC_INIT(rgbOdomScan2d),
+		SYNC_INIT(rgbOdomScan3d),
+		SYNC_INIT(rgbOdomInfo),
+		SYNC_INIT(rgbOdomScan2dInfo),
+		SYNC_INIT(rgbOdomScan3dInfo),
+
+		// RGB-only + User Data
+		SYNC_INIT(rgbData),
+		SYNC_INIT(rgbDataScan2d),
+		SYNC_INIT(rgbDataScan3d),
+		SYNC_INIT(rgbDataInfo),
+		SYNC_INIT(rgbDataScan2dInfo),
+		SYNC_INIT(rgbDataScan3dInfo),
+
+		// RGB-only + Odom + User Data
+		SYNC_INIT(rgbOdomData),
+		SYNC_INIT(rgbOdomDataScan2d),
+		SYNC_INIT(rgbOdomDataScan3d),
+		SYNC_INIT(rgbOdomDataInfo),
+		SYNC_INIT(rgbOdomDataScan2dInfo),
+		SYNC_INIT(rgbOdomDataScan3dInfo),
+
 		// 1 RGBD
 		SYNC_INIT(rgbdScan2d),
 		SYNC_INIT(rgbdScan3d),
@@ -228,17 +262,27 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(odomDataScan2d),
 		SYNC_INIT(odomDataScan3d),
 		SYNC_INIT(odomDataScan2dInfo),
-		SYNC_INIT(odomDataScan3dInfo)
+		SYNC_INIT(odomDataScan3dInfo),
+
+		// Odom
+		SYNC_INIT(odomInfo),
+		// Odom + User Data
+		SYNC_INIT(odomData),
+		SYNC_INIT(odomDataInfo)
 
 {
 }
 
-void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name)
+void CommonDataSubscriber::setupCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		const std::string & name)
 {
 	bool subscribeScan2d = false;
 	bool subscribeScan3d = false;
 	bool subscribeOdomInfo = false;
 	bool subscribeUserData = false;
+	bool subscribeOdom = true;
 	int rgbdCameras = 1;
 	name_ = name;
 
@@ -248,12 +292,24 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 	{
 		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_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);
+	if(subscribedToDepth_ && subscribedToRGB_)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgb cannot be true at the same time. Parameters subscribe_rgb is set to false.");
+		subscribedToRGB_ = false;
+	}
+	if(subscribedToStereo_ && subscribedToRGB_)
+	{
+		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_ && subscribedToStereo_)
 	{
 		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
@@ -276,7 +332,7 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 	}
 	if(subscribeScan2d || subscribeScan3d)
 	{
-		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_)
+		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
 		{
 			approxSync_ = false; // default for scan: exact sync
 		}
@@ -315,13 +371,13 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 	ROS_INFO("%s: rgbd_cameras = %d", name.c_str(), rgbdCameras);
 	ROS_INFO("%s: approx_sync   = %s", name.c_str(), approxSync_?"true":"false");
 
-	bool subscribeOdom = odomFrameId.empty();
+	subscribedToOdom_ = odomFrameId.empty() && subscribeOdom;
 	if(subscribedToDepth_)
 	{
 		setupDepthCallbacks(
 				nh,
 				pnh,
-				subscribeOdom,
+				subscribedToOdom_,
 				subscribeUserData,
 				subscribeScan2d,
 				subscribeScan3d,
@@ -334,7 +390,20 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 		setupStereoCallbacks(
 				nh,
 				pnh,
-				subscribeOdom,
+				subscribedToOdom_,
+				subscribeOdomInfo,
+				queueSize_,
+				approxSync_);
+	}
+	else if(subscribedToRGB_)
+	{
+		setupRGBCallbacks(
+				nh,
+				pnh,
+				subscribedToOdom_,
+				subscribeUserData,
+				subscribeScan2d,
+				subscribeScan3d,
 				subscribeOdomInfo,
 				queueSize_,
 				approxSync_);
@@ -346,7 +415,7 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 			setupRGBD4Callbacks(
 					nh,
 					pnh,
-					subscribeOdom,
+					subscribedToOdom_,
 					subscribeUserData,
 					subscribeScan2d,
 					subscribeScan3d,
@@ -359,7 +428,7 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 			setupRGBD3Callbacks(
 					nh,
 					pnh,
-					subscribeOdom,
+					subscribedToOdom_,
 					subscribeUserData,
 					subscribeScan2d,
 					subscribeScan3d,
@@ -372,7 +441,7 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 			setupRGBD2Callbacks(
 					nh,
 					pnh,
-					subscribeOdom,
+					subscribedToOdom_,
 					subscribeUserData,
 					subscribeScan2d,
 					subscribeScan3d,
@@ -385,7 +454,7 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 			setupRGBDCallbacks(
 					nh,
 					pnh,
-					subscribeOdom,
+					subscribedToOdom_,
 					subscribeUserData,
 					subscribeScan2d,
 					subscribeScan3d,
@@ -400,14 +469,24 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 					nh,
 					pnh,
 					subscribeScan2d,
-					subscribeOdom,
+					subscribedToOdom_,
+					subscribeUserData,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync_);
+	}
+	else if(subscribedToOdom_)
+	{
+		setupOdomCallbacks(
+					nh,
+					pnh,
 					subscribeUserData,
 					subscribeOdomInfo,
 					queueSize_,
 					approxSync_);
 	}
 
-	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_)
+	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToRGB_ || subscribedToOdom_)
 	{
 		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
 		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
@@ -463,6 +542,38 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(stereoOdom);
 	SYNC_DEL(stereoOdomInfo);
 
+	// RGB-only
+	SYNC_DEL(rgb);
+	SYNC_DEL(rgbScan2d);
+	SYNC_DEL(rgbScan3d);
+	SYNC_DEL(rgbInfo);
+	SYNC_DEL(rgbScan2dInfo);
+	SYNC_DEL(rgbScan3dInfo);
+
+	// RGB-only + Odom
+	SYNC_DEL(rgbOdom);
+	SYNC_DEL(rgbOdomScan2d);
+	SYNC_DEL(rgbOdomScan3d);
+	SYNC_DEL(rgbOdomInfo);
+	SYNC_DEL(rgbOdomScan2dInfo);
+	SYNC_DEL(rgbOdomScan3dInfo);
+
+	// RGB-only + User Data
+	SYNC_DEL(rgbData);
+	SYNC_DEL(rgbDataScan2d);
+	SYNC_DEL(rgbDataScan3d);
+	SYNC_DEL(rgbDataInfo);
+	SYNC_DEL(rgbDataScan2dInfo);
+	SYNC_DEL(rgbDataScan3dInfo);
+
+	// RGB-only + Odom + User Data
+	SYNC_DEL(rgbOdomData);
+	SYNC_DEL(rgbOdomDataScan2d);
+	SYNC_DEL(rgbOdomDataScan3d);
+	SYNC_DEL(rgbOdomDataInfo);
+	SYNC_DEL(rgbOdomDataScan2dInfo);
+	SYNC_DEL(rgbOdomDataScan3dInfo);
+
 	// 1 RGBD
 	SYNC_DEL(rgbdScan2d);
 	SYNC_DEL(rgbdScan3d);
@@ -612,6 +723,13 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(odomDataScan2dInfo);
 	SYNC_DEL(odomDataScan3dInfo);
 
+	// Odom
+	SYNC_DEL(odomInfo);
+	// Odom + User Data
+	SYNC_DEL(odomData);
+	SYNC_DEL(odomDataInfo);
+
+
 	for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
 	{
 		delete rgbdSubs_[i];
@@ -625,6 +743,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	pnh.deleteParam("subscribe_scan");
 	pnh.deleteParam("subscribe_scan_cloud");
 	pnh.deleteParam("subscribe_stereo");
+	pnh.deleteParam("subscribe_rgb");
 	pnh.deleteParam("subscribe_rgbd");
 	pnh.deleteParam("subscribe_odom_info");
 	pnh.deleteParam("subscribe_user_data");

src/CoreWrapper.cpp

@@ -108,8 +108,6 @@ CoreWrapper::CoreWrapper() :
 		mapToOdom_(rtabmap::Transform::getIdentity()),
 		transformThread_(0),
 		tfThreadRunning_(false),
-		SYNC_INIT(rgb),
-		SYNC_INIT(rgbOdom),
 		stereoToDepth_(false),
 		odomSensorSync_(false),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
@@ -592,98 +590,49 @@ void CoreWrapper::onInit()
 	if(!this->isDataSubscribed())
 	{
 		bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
-		bool incremental = uStr2Bool(parameters_.at(Parameters::kMemIncrementalMemory()).c_str());
-		if(isRGBD && !incremental)
+		if(isRGBD)
 		{
-			NODELET_INFO("\"%s\" is true and \"%s\" is false, subscribing to RGB + camera info...",
-					Parameters::kRGBDEnabled().c_str(),
-					Parameters::kMemIncrementalMemory().c_str());
-			ros::NodeHandle rgb_nh(nh, "rgb");
-			ros::NodeHandle rgb_pnh(pnh, "rgb");
-			image_transport::ImageTransport rgb_it(rgb_nh);
-			image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
-			rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-			rgbCameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
-
-			std::string odomFrameId;
-			pnh.getParam("odom_frame_id", odomFrameId);
-			if(!odomFrameId.empty())
-			{
-				// odom from TF
-				if(isApproxSync())
-				{
-					rgbApproximateSync_ = new message_filters::Synchronizer<rgbApproximateSyncPolicy>(
-							rgbApproximateSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_);
-					rgbApproximateSync_->registerCallback(boost::bind(&CoreWrapper::rgbCallback, this, _1, _2));
-				}
-				else
-				{
-					rgbExactSync_ = new message_filters::Synchronizer<rgbExactSyncPolicy>(
-							rgbExactSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_);
-					rgbExactSync_->registerCallback(boost::bind(&CoreWrapper::rgbCallback, this, _1, _2));
-				}
-				NODELET_INFO("\n%s subscribed to:\n   %s\n   %s",
-						getName().c_str(),
-						rgbSub_.getTopic().c_str(),
-						rgbCameraInfoSub_.getTopic().c_str());
-			}
-			else
-			{
-				rgbOdomSub_.subscribe(nh, "odom", 1);
-				if(isApproxSync())
-				{
-					rgbOdomApproximateSync_ = new message_filters::Synchronizer<rgbOdomApproximateSyncPolicy>(
-							rgbOdomApproximateSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_, rgbOdomSub_);
-					rgbOdomApproximateSync_->registerCallback(boost::bind(&CoreWrapper::rgbOdomCallback, this, _1, _2, _3));
-				}
-				else
-				{
-					rgbOdomExactSync_ = new message_filters::Synchronizer<rgbOdomExactSyncPolicy>(
-							rgbOdomExactSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_, rgbOdomSub_);
-					rgbOdomExactSync_->registerCallback(boost::bind(&CoreWrapper::rgbOdomCallback, this, _1, _2, _3));
-				}
-				NODELET_INFO("\n%s subscribed to:\n   %s\n   %s\n   %s",
-						getName().c_str(),
-						rgbSub_.getTopic().c_str(),
-						rgbCameraInfoSub_.getTopic().c_str(),
-						rgbOdomSub_.getTopic().c_str());
-			}
+			NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
+					  "parameter \"%s\" is true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
+					  "to true to use rtabmap node for RGB-D SLAM, set \"%s\" to false for loop closure "
+					  "detection on images-only or set subscribe_rgb to true to localize a single RGB camera against pre-built 3D map.",
+					  Parameters::kRGBDEnabled().c_str(),
+					  Parameters::kRGBDEnabled().c_str());
 		}
-		else
-		{
-			if(isRGBD)
-			{
-				NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
-						  "parameter \"%s\" and \"%s\" are true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
-						  "to true to use rtabmap node for RGB-D SLAM, set \"%s\" to false for loop closure "
-						  "detection on images-only or set \"%s\" to false to localize a single RGB camera against pre-built 3D map.",
-						  Parameters::kRGBDEnabled().c_str(),
-						  Parameters::kMemIncrementalMemory().c_str(),
-						  Parameters::kRGBDEnabled().c_str(),
-						  Parameters::kMemIncrementalMemory().c_str());
-			}
-			ros::NodeHandle rgb_nh(nh, "rgb");
-			ros::NodeHandle rgb_pnh(pnh, "rgb");
-			image_transport::ImageTransport rgb_it(rgb_nh);
-			image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
-			defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
 
-			NODELET_INFO("\n%s subscribed to:\n   %s", getName().c_str(), defaultSub_.getTopic().c_str());
-		}
+		NODELET_INFO("\n%s subscribed to:\n   %s", getName().c_str(), defaultSub_.getTopic().c_str());
 	}
-	else if(!this->isSubscribedToDepth() && !this->isSubscribedToStereo() && !this->isSubscribedToRGBD() &&
-			(this->isSubscribedToScan2d() || this->isSubscribedToScan3d()))
+	else if(!this->isSubscribedToDepth() &&
+			!this->isSubscribedToStereo() &&
+			!this->isSubscribedToRGBD() &&
+			!this->isSubscribedToRGB() &&
+			(this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || this->isSubscribedToOdom()))
 	{
-		ROS_WARN("Subscribing to laser scan but no image subscription is enabled, bag-of-words loop closure detection will be disabled...");
+		ROS_WARN("There is no image subscription, bag-of-words loop closure detection will be disabled...");
 		int kpMaxFeatures = Parameters::defaultKpMaxFeatures();
 		int registrationStrategy = Parameters::defaultRegStrategy();
 		Parameters::parse(parameters_, Parameters::kKpMaxFeatures(), kpMaxFeatures);
 		Parameters::parse(parameters_, Parameters::kRegStrategy(), registrationStrategy);
-		if(kpMaxFeatures!= -1 || registrationStrategy != 1)
+		bool updateParams = false;
+		if(kpMaxFeatures!= -1)
 		{
 			uInsert(parameters_, ParametersPair(Parameters::kKpMaxFeatures(), "-1"));
+			ROS_WARN("Setting %s=-1 (bag-of-words disabled)", Parameters::kKpMaxFeatures().c_str());
+			updateParams = true;
+		}
+		if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d()) && registrationStrategy != 1)
+		{
 			uInsert(parameters_, ParametersPair(Parameters::kRegStrategy(), "1"));
-			ROS_WARN("Setting %s=-1 (bag-of-words disabled) and %s=1 (ICP)", Parameters::kKpMaxFeatures().c_str(), Parameters::kRegStrategy().c_str());
+			ROS_WARN("Setting %s=1 (ICP)", Parameters::kRegStrategy().c_str());
+			updateParams = true;
+		}
+		if(updateParams)
+		{
 			rtabmap_.parseParameters(parameters_);
 		}
 	}
@@ -705,9 +654,6 @@ CoreWrapper::~CoreWrapper()
 		delete transformThread_;
 	}
 
-	SYNC_DEL(rgb);
-	SYNC_DEL(rgbOdom);
-
 	this->saveParameters(configPath_);
 
 	ros::NodeHandle nh;
@@ -859,33 +805,6 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
 	}
 }
 
-
-void CoreWrapper::rgbCallback(
-		const sensor_msgs::ImageConstPtr& imageMsg,
-		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
-{
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
-}
-
-void CoreWrapper::rgbOdomCallback(
-		const sensor_msgs::ImageConstPtr& imageMsg,
-		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-		const nav_msgs::OdometryConstPtr & odomMsg)
-{
-	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
-	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
-}
-
 bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Time stamp)
 {
 	if(!paused_)
@@ -1850,6 +1769,141 @@ void CoreWrapper::commonLaserScanCallback(
 	covariance_ = cv::Mat();
 }
 
+void CoreWrapper::commonOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	UASSERT(odomMsg.get());
+	std::string odomFrameId = odomFrameId_;
+
+	odomFrameId = odomMsg->header.frame_id;
+	if(!odomUpdate(odomMsg, odomMsg->header.stamp))
+	{
+		return;
+	}
+
+	Transform groundTruthPose;
+	if(!groundTruthFrameId_.empty())
+	{
+		groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
+	}
+
+	cv::Mat userData;
+	if(userDataMsg.get())
+	{
+		userData = rtabmap_ros::userDataFromROS(*userDataMsg);
+		UScopeMutex lock(userDataMutex_);
+		if(!userData_.empty())
+		{
+			NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
+			userData_ = cv::Mat();
+		}
+	}
+	else
+	{
+		UScopeMutex lock(userDataMutex_);
+		userData = userData_;
+		userData_ = cv::Mat();
+	}
+
+	cv::Mat rgb = cv::Mat::zeros(2,1,CV_8UC1);
+	cv::Mat depth = cv::Mat::zeros(2,1,CV_16UC1);
+	CameraModel model(
+			1,
+			1,
+			0.5,
+			1,
+			Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			0,
+			cv::Size(1,2));
+
+	SensorData data(
+			rgb,
+			depth,
+			model,
+			lastPoseIntermediate_?-1:odomMsg->header.seq,
+			rtabmap_ros::timestampFromROS(lastPoseStamp_),
+			userData);
+	data.setGroundTruth(groundTruthPose);
+
+	//global pose
+	if(!globalPose_.header.stamp.isZero())
+	{
+		// assume sensor is fixed
+		Transform sensorToBase = rtabmap_ros::getTransform(
+				globalPose_.header.frame_id,
+				frameId_,
+				lastPoseStamp_,
+				tfListener_,
+				waitForTransform_?waitForTransformDuration_:0.0);
+		if(!sensorToBase.isNull())
+		{
+			Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
+			globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
+
+			// Correction of the global pose accounting the odometry movement since we received it
+			Transform correction = rtabmap_ros::getTransform(
+					frameId_,
+					odomFrameId,
+					globalPose_.header.stamp,
+					lastPoseStamp_,
+					tfListener_,
+					waitForTransform_?waitForTransformDuration_:0.0);
+			if(!correction.isNull())
+			{
+				globalPose *= correction;
+			}
+			else
+			{
+				NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
+						"If odometry is small since it received the global pose and "
+						"covariance is large, this should not be a problem.");
+			}
+			cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
+			data.setGlobalPose(globalPose, globalPoseCovariance);
+		}
+	}
+	globalPose_.header.stamp = ros::Time(0);
+
+	if(gps_.stamp() > 0.0)
+	{
+		data.setGPS(gps_);
+	}
+	gps_ = rtabmap::GPS();
+
+	OdometryInfo odomInfo;
+	if(odomInfoMsg.get())
+	{
+		odomInfo = odomInfoFromROS(*odomInfoMsg);
+	}
+
+	//tag detections
+	Landmarks landmarks = rtabmap_ros::landmarksFromROS(
+			tags_,
+			frameId_,
+			odomFrameId,
+			lastPoseStamp_,
+			tfListener_,
+			waitForTransform_?waitForTransformDuration_:0,
+			landmarkDefaultLinVariance_,
+			landmarkDefaultAngVariance_);
+	tags_.clear();
+	if(!landmarks.empty())
+	{
+		data.setLandmarks(landmarks);
+	}
+
+	process(lastPoseStamp_,
+			data,
+			lastPose_,
+			odomFrameId,
+			covariance_,
+			odomInfo);
+
+	covariance_ = cv::Mat();
+}
+
 void CoreWrapper::process(
 		const ros::Time & stamp,
 		const SensorData & data,

src/GuiWrapper.cpp

@@ -174,14 +174,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this);
 
 	setupCallbacks(nh, pnh, ros::this_node::getName()); // do it at the end
-	if(!this->isDataSubscribed())
-	{
-		defaultSub_ = nh.subscribe("odom", queueSize_, &GuiWrapper::defaultCallback, this);
-
-		ROS_INFO("\n%s subscribed to:\n   %s",
-				ros::this_node::getName().c_str(),
-				defaultSub_.getTopic().c_str());
-	}
 }
 
 GuiWrapper::~GuiWrapper()
@@ -437,7 +429,7 @@ void GuiWrapper::commonDepthCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
-	UASSERT(imageMsgs.size() == 0 || (imageMsgs.size() == depthMsgs.size() && imageMsgs.size() == cameraInfoMsgs.size()));
+	UASSERT(imageMsgs.size() == 0 || (imageMsgs.size() == cameraInfoMsgs.size()));
 
 	std_msgs::Header odomHeader;
 	if(odomMsg.get())
@@ -506,7 +498,7 @@ void GuiWrapper::commonDepthCallback(
 	{
 		lastOdomInfoUpdateTime_ = UTimer::now();
 
-		if(imageMsgs.size() && imageMsgs[0].get() && depthMsgs[0].get())
+		if(imageMsgs.size() && imageMsgs[0].get() && depthMsgs.size() && depthMsgs[0].get())
 		{
 			if(!rtabmap_ros::convertRGBDMsgs(
 					imageMsgs,
@@ -909,19 +901,87 @@ void GuiWrapper::commonLaserScanCallback(
 	QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
 }
 
-// With odom msg
-void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
+void GuiWrapper::commonOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
-	this->commonSingleDepthCallback(
-			odomMsg,
-			rtabmap_ros::UserDataConstPtr(),
-			cv_bridge::CvImageConstPtr(),
-			cv_bridge::CvImageConstPtr(),
-			sensor_msgs::CameraInfo(),
-			sensor_msgs::CameraInfo(),
-			sensor_msgs::LaserScanConstPtr(),
-			sensor_msgs::PointCloud2ConstPtr(),
-			rtabmap_ros::OdomInfoConstPtr());
+	UASSERT(odomMsg.get());
+
+	std_msgs::Header odomHeader = odomMsg->header;
+
+	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+	if(odomMsg.get())
+	{
+		UASSERT(odomMsg->twist.covariance.size() == 36);
+		if(odomMsg->twist.covariance[0] != 0 &&
+			 odomMsg->twist.covariance[7] != 0 &&
+			 odomMsg->twist.covariance[14] != 0 &&
+			 odomMsg->twist.covariance[21] != 0 &&
+			 odomMsg->twist.covariance[28] != 0 &&
+			 odomMsg->twist.covariance[35] != 0)
+		{
+			covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
+		}
+	}
+	if(odomHeader.frame_id.empty())
+	{
+		ROS_ERROR("Odometry frame not set!?");
+		return;
+	}
+
+	rtabmap::OdometryInfo info;
+	bool ignoreData = false;
+
+	// limit update rate
+	if(maxOdomUpdateRate_<=0.0 ||
+	   (UTimer::now() - lastOdomInfoUpdateTime_ > 1.0/maxOdomUpdateRate_ &&
+	   !mainWindow_->isProcessingOdometry() &&
+	   !mainWindow_->isProcessingStatistics()))
+	{
+		lastOdomInfoUpdateTime_ = UTimer::now();
+
+		if(odomInfoMsg.get())
+		{
+			info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
+		}
+		ignoreData = false;
+	}
+	else if(odomInfoMsg.get())
+	{
+		info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
+		ignoreData = true;
+	}
+	else
+	{
+		// don't update GUI odom stuff if we don't use visual odometry
+		return;
+	}
+
+	cv::Mat rgb = cv::Mat::zeros(2,1,CV_8UC1);
+	cv::Mat depth = cv::Mat::zeros(2,1,CV_16UC1);
+	CameraModel model(
+			1,
+			1,
+			0.5,
+			1,
+			Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			0,
+			cv::Size(1,2));
+
+	info.reg.covariance = covariance;
+	rtabmap::OdometryEvent odomEvent(
+		rtabmap::SensorData(
+				rgb,
+				depth,
+				model,
+				odomHeader.seq,
+				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
+		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
+		info);
+
+	QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
 }
 
 }

src/impl/CommonDataSubscriberOdom.cpp

@@ -0,0 +1,113 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_ros/CommonDataSubscriber.h>
+
+namespace rtabmap_ros {
+
+void CommonDataSubscriber::odomCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg)
+{
+	callbackCalled();
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupOdomCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeUserData,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup scan callback");
+
+	if(subscribeUserData || subscribeOdomInfo)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+
+		if(subscribeUserData)
+		{
+			userDataSub_.subscribe(nh, "user_data", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL3(odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(odomData, approxSync, queueSize, odomSub_, userDataSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL2(odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
+		}
+	}
+	else
+	{
+		odomSubOnly_ = nh.subscribe("odom", 1, &CommonDataSubscriber::odomCallback, this);
+		subscribedTopicsMsg_ =
+				uFormat("\n%s subscribed to:\n   %s",
+				ros::this_node::getName().c_str(),
+				odomSubOnly_.getTopic().c_str());
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGB.cpp

@@ -0,0 +1,532 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_ros/CommonDataSubscriber.h>
+
+namespace rtabmap_ros {
+
+// RGB
+void CommonDataSubscriber::rgbCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan2dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan3dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan2dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan3dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+// RGB + Odom
+void CommonDataSubscriber::rgbOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+// RGB + Depth + User Data
+void CommonDataSubscriber::rgbDataCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan2dInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan3dInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+// RGB + Depth + Odom + User Data
+void CommonDataSubscriber::rgbOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupRGBCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgb-only callback");
+
+	std::string rgbPrefix = "rgb";
+	ros::NodeHandle rgb_nh(nh, rgbPrefix);
+	ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
+	image_transport::ImageTransport rgb_it(rgb_nh);
+	image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+
+	imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+	cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+		userDataSub_.subscribe(nh, "user_data", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+	else if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+	else
+	{
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL3(rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */