Split librtabmap_ros.so in to 3 libraries to decrease compilation time. Added scan-only common subscribers.

src/CommonDataSubscriber.cpp

@@ -206,7 +206,29 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd4OdomDataScan3d),
 		SYNC_INIT(rgbd4OdomDataInfo),
 		SYNC_INIT(rgbd4OdomDataScan2dInfo),
-		SYNC_INIT(rgbd4OdomDataScan3dInfo)
+		SYNC_INIT(rgbd4OdomDataScan3dInfo),
+
+		// Scan
+		SYNC_INIT(scan2dInfo),
+		SYNC_INIT(scan3dInfo),
+
+		// Scan + Odom
+		SYNC_INIT(odomScan2d),
+		SYNC_INIT(odomScan3d),
+		SYNC_INIT(odomScan2dInfo),
+		SYNC_INIT(odomScan3dInfo),
+
+		// Scan + User Data
+		SYNC_INIT(dataScan2d),
+		SYNC_INIT(dataScan3d),
+		SYNC_INIT(dataScan2dInfo),
+		SYNC_INIT(dataScan3dInfo),
+
+		// Scan + Odom + User Data
+		SYNC_INIT(odomDataScan2d),
+		SYNC_INIT(odomDataScan3d),
+		SYNC_INIT(odomDataScan2dInfo),
+		SYNC_INIT(odomDataScan3dInfo)
 
 {
 }
@@ -256,8 +278,7 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 	{
 		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_)
 		{
-			ROS_WARN("When subscribing to laser scan, you should subscribe to depth, stereo or rgbd too. Subscribing to depth by default...");
-			subscribedToDepth_ = true;
+			approxSync_ = false; // default for scan: exact sync
 		}
 	}
 	if(subscribedToStereo_)
@@ -373,7 +394,20 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 					approxSync_);
 		}
 	}
-	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_)
+	else if(subscribeScan2d || subscribeScan3d)
+	{
+		setupScanCallbacks(
+					nh,
+					pnh,
+					subscribeScan2d,
+					subscribeOdom,
+					subscribeUserData,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync_);
+	}
+
+	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_)
 	{
 		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
 		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
@@ -556,6 +590,28 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd4OdomDataScan2dInfo);
 	SYNC_DEL(rgbd4OdomDataScan3dInfo);
 
+	// Scan
+	SYNC_DEL(scan2dInfo);
+	SYNC_DEL(scan3dInfo);
+
+	// Scan + Odom
+	SYNC_DEL(odomScan2d);
+	SYNC_DEL(odomScan3d);
+	SYNC_DEL(odomScan2dInfo);
+	SYNC_DEL(odomScan3dInfo);
+
+	// Scan + User Data
+	SYNC_DEL(dataScan2d);
+	SYNC_DEL(dataScan3d);
+	SYNC_DEL(dataScan2dInfo);
+	SYNC_DEL(dataScan3dInfo);
+
+	// Scan + Odom + User Data
+	SYNC_DEL(odomDataScan2d);
+	SYNC_DEL(odomDataScan3d);
+	SYNC_DEL(odomDataScan2dInfo);
+	SYNC_DEL(odomDataScan3dInfo);
+
 	for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
 	{
 		delete rgbdSubs_[i];
@@ -620,8 +676,6 @@ void CommonDataSubscriber::commonSingleDepthCallback(
 	{
 		commonStereoCallback(odomMsg, userDataMsg, imageMsg, depthMsg, rgbCameraInfoMsg, depthCameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 	}
-
-
 }
 
 } /* namespace rtabmap_ros */

src/CoreWrapper.cpp

@@ -666,6 +666,22 @@ void CoreWrapper::onInit()
 			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()))
+	{
+		ROS_WARN("Subscribing to laser scan but no image subscription is enabled, 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)
+		{
+			uInsert(parameters_, ParametersPair(Parameters::kKpMaxFeatures(), "-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());
+			rtabmap_.parseParameters(parameters_);
+		}
+	}
 
 	userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
 	globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this);
@@ -1564,6 +1580,220 @@ void CoreWrapper::commonStereoCallback(
 	covariance_ = cv::Mat();
 }
 
+void CoreWrapper::commonLaserScanCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	UASSERT(scan2dMsg.get() || scan3dMsg.get());
+	std::string odomFrameId = odomFrameId_;
+	if(odomMsg.get())
+	{
+		odomFrameId = odomMsg->header.frame_id;
+		if(scan2dMsg.get())
+		{
+			if(!odomUpdate(odomMsg, scan2dMsg->header.stamp))
+			{
+				return;
+			}
+		}
+		else if(scan3dMsg.get())
+		{
+			if(!odomUpdate(odomMsg, scan3dMsg->header.stamp))
+			{
+				return;
+			}
+		}
+		else
+		{
+			return;
+		}
+	}
+	else if(scan2dMsg.get())
+	{
+		if(!odomTFUpdate(scan2dMsg->header.stamp))
+		{
+			return;
+		}
+	}
+	else if(scan3dMsg.get())
+	{
+		if(!odomTFUpdate(scan3dMsg->header.stamp))
+		{
+			return;
+		}
+	}
+	else
+	{
+		return;
+	}
+
+	cv::Mat scan;
+	Transform scanLocalTransform = Transform::getIdentity();
+	if(scan2dMsg.get() != 0)
+	{
+		if(!rtabmap_ros::convertScanMsg(
+				scan2dMsg,
+				frameId_,
+				odomSensorSync_?odomFrameId:"",
+				lastPoseStamp_,
+				scan,
+				scanLocalTransform,
+				tfListener_,
+				waitForTransform_?waitForTransformDuration_:0))
+		{
+			NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
+			return;
+		}
+		Transform zAxis(0,0,1,0,0,0);
+		if((scanLocalTransform.rotation()*zAxis).z() < 0)
+		{
+			cv::Mat flipScan;
+			cv::flip(scan, flipScan, 1);
+			scan = flipScan;
+		}
+		if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
+		{
+			// backward compatibility, project 2D scan in /base_link frame
+			scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
+			scanLocalTransform = Transform::getIdentity();
+		}
+	}
+	else if(scan3dMsg.get() != 0)
+	{
+		if(!rtabmap_ros::convertScan3dMsg(
+				scan3dMsg,
+				frameId_,
+				odomSensorSync_?odomFrameId:"",
+				lastPoseStamp_,
+				scan,
+				scanLocalTransform,
+				tfListener_,
+				waitForTransform_?waitForTransformDuration_:0))
+		{
+			NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
+			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::getIdentity(),
+			0,
+			cv::Size(1,2));
+
+	SensorData data(scan2dMsg.get() != 0?
+					LaserScan::backwardCompatibility(scan,
+							scan2dMsg->range_min,
+							scan2dMsg->range_max,
+							scan2dMsg->angle_min,
+							scan2dMsg->angle_max,
+							scan2dMsg->angle_increment,
+							scanLocalTransform):
+					LaserScan::backwardCompatibility(scan,
+							scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
+							0,
+							scanLocalTransform),
+			rgb,
+			depth,
+			model,
+			lastPoseIntermediate_?-1:scan2dMsg.get() != 0?scan2dMsg->header.seq:scan3dMsg->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);
+	}
+
+	process(lastPoseStamp_,
+			data,
+			lastPose_,
+			odomFrameId,
+			covariance_,
+			odomInfo);
+
+	covariance_ = cv::Mat();
+}
+
 void CoreWrapper::process(
 		const ros::Time & stamp,
 		const SensorData & data,

src/GuiWrapper.cpp

@@ -745,6 +745,149 @@ void GuiWrapper::commonStereoCallback(
 	QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
 }
 
+void GuiWrapper::commonLaserScanCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	UASSERT(scan2dMsg.get() || scan3dMsg.get());
+
+	std_msgs::Header odomHeader;
+	if(odomMsg.get())
+	{
+		odomHeader = odomMsg->header;
+	}
+	else
+	{
+		if(scan2dMsg.get())
+		{
+			odomHeader = scan2dMsg->header;
+		}
+		else if(scan3dMsg.get())
+		{
+			odomHeader = scan3dMsg->header;
+		}
+		else
+		{
+			return;
+		}
+		odomHeader.frame_id = odomFrameId_;
+	}
+
+	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;
+	}
+
+	cv::Mat scan;
+	Transform scanLocalTransform = Transform::getIdentity();
+	rtabmap::OdometryInfo info;
+	bool ignoreData = false;
+
+	// limit 10 Hz max
+	if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
+	   !mainWindow_->isProcessingOdometry() &&
+	   !mainWindow_->isProcessingStatistics())
+	{
+		lastOdomInfoUpdateTime_ = UTimer::now();
+
+		if(scan2dMsg.get() != 0)
+		{
+			if(!rtabmap_ros::convertScanMsg(
+					scan2dMsg,
+					frameId_,
+					odomSensorSync_?odomHeader.frame_id:"",
+					odomHeader.stamp,
+					scan,
+					scanLocalTransform,
+					tfListener_,
+					waitForTransform_?waitForTransformDuration_:0))
+			{
+				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update...");
+				return;
+			}
+		}
+		else if(scan3dMsg.get() != 0)
+		{
+			if(!rtabmap_ros::convertScan3dMsg(
+					scan3dMsg,
+					frameId_,
+					odomSensorSync_?odomHeader.frame_id:"",
+					odomHeader.stamp,
+					scan,
+					scanLocalTransform,
+					tfListener_,
+					waitForTransform_?waitForTransformDuration_:0))
+			{
+				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
+				return;
+			}
+		}
+
+		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::getIdentity(),
+			0,
+			cv::Size(1,2));
+
+	info.reg.covariance = covariance;
+	rtabmap::OdometryEvent odomEvent(
+		rtabmap::SensorData(
+				LaserScan::backwardCompatibility(scan,
+						scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
+						scan2dMsg.get()?(int)scan2dMsg->range_max:0,
+						scanLocalTransform),
+				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));
+}
+
 // With odom msg
 void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
 {

src/OdometryROS.cpp

@@ -192,6 +192,17 @@ void OdometryROS::onInit()
 
 	//parameters
 	parameters_ = Parameters::getDefaultOdometryParameters(stereoParams_, visParams_, icpParams_);
+	if(icpParams_)
+	{
+		if(!visParams_)
+		{
+			uInsert(parameters_, ParametersPair(Parameters::kRegStrategy(), "1"));
+		}
+		else
+		{
+			uInsert(parameters_, ParametersPair(Parameters::kRegStrategy(), "2"));
+		}
+	}
 	parameters_.insert(*Parameters::getDefaultParameters().find(Parameters::kRtabmapImagesAlreadyRectified()));
 	if(!configPath.empty())
 	{

src/impl/CommonDataSubscriberScan.cpp

@@ -0,0 +1,360 @@
+/*
+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::scan2dCallback(
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scan3dCallback(
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scan2dInfoCallback(
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scan3dInfoCallback(
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::odomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::dataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScan2dInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScan3dInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::odomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	callbackCalled();
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	callbackCalled();
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupScanCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool scan2dTopic,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup scan callback");
+
+	if(subscribeOdom || subscribeUserData || subscribeOdomInfo)
+	{
+		if(scan2dTopic)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+		}
+		else
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+		}
+
+		if(subscribeOdom && subscribeUserData)
+		{
+			odomSub_.subscribe(nh, "odom", 1);
+			userDataSub_.subscribe(nh, "user_data", 1);
+
+			if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
+				}
+			}
+			else
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
+				}
+			}
+		}
+		else if(subscribeOdom)
+		{
+			odomSub_.subscribe(nh, "odom", 1);
+
+			if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
+				}
+			}
+			else
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
+				}
+			}
+		}
+		else if(subscribeUserData)
+		{
+			userDataSub_.subscribe(nh, "user_data", 1);
+
+			if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
+				}
+			}
+			else
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
+				}
+			}
+		}
+		else
+		{
+			if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL2(scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
+				}
+			}
+			else
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL2(scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
+				}
+			}
+		}
+	}
+	else
+	{
+		if(scan2dTopic)
+		{
+			subscribedToScan2d_ = true;
+			scan2dSubOnly_ = nh.subscribe("scan", 1, &CommonDataSubscriber::scan2dCallback, this);
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					ros::this_node::getName().c_str(),
+					scan2dSubOnly_.getTopic().c_str());
+		}
+		else
+		{
+			subscribedToScan3d_ = true;
+			scan3dSubOnly_ = nh.subscribe("scan_cloud", 1, &CommonDataSubscriber::scan3dCallback, this);
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					ros::this_node::getName().c_str(),
+					scan3dSubOnly_.getTopic().c_str());
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */