Added "scan_cloud" topic (PointCloud2 type) with "subscribe_scan_cloud" parameter to rtabmap and rtabmapviz nodes.

src/CoreWrapper.cpp

@@ -109,7 +109,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	ros::NodeHandle nh;
 	ros::NodeHandle pnh("~");
 
-	bool subscribeLaserScan = false;
+	bool subscribeScan2d = false;
+	bool subscribeScan3d = false;
 	bool subscribeDepth = true;
 	bool subscribeStereo = false;
 	int depthCameras = 1;
@@ -121,14 +122,24 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	// ROS related parameters (private)
 	pnh.param("subscribe_depth",     subscribeDepth, subscribeDepth);
-	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+	if(pnh.getParam("subscribe_laserScan", subscribeScan2d))
+	{
+		ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
+	}
+	pnh.param("subscribe_scan",      subscribeScan2d, subscribeScan2d);
+	pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
 	pnh.param("subscribe_stereo",    subscribeStereo, subscribeStereo);
 	if(subscribeDepth && subscribeStereo)
 	{
 		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
 		subscribeDepth = false;
 	}
-	if(subscribeLaserScan)
+	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.");
+		subscribeDepth = false;
+	}
+	if(subscribeScan2d || subscribeScan3d)
 	{
 		if(!subscribeDepth && !subscribeStereo)
 		{
@@ -369,7 +380,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this);
 	setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this);
 
-	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync, depthCameras);
+	setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras);
 
 	int optimizeIterations = 0;
 	Parameters::parse(parameters_, Parameters::kRGBDOptimizeIterations(), optimizeIterations);
@@ -453,7 +464,7 @@ ParametersMap CoreWrapper::loadParameters(const std::string & configFile)
 		{
 			ROS_WARN("Config file doesn't exist! It will be generated...");
 		}
-		Rtabmap::readParameters(configFile.c_str(), parameters);
+		Parameters::readINI(configFile.c_str(), parameters);
 	}
 	// otherwise take default parameters
 
@@ -470,7 +481,7 @@ void CoreWrapper::saveParameters(const std::string & configFile)
 		{
 			printf("Config file doesn't exist, a new one will be created.\n");
 		}
-		Rtabmap::writeParameters(configFile.c_str(), parameters_);
+		Parameters::writeINI(configFile.c_str(), parameters_);
 	}
 	else
 	{
@@ -670,7 +681,8 @@ void CoreWrapper::commonDepthCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
 		const sensor_msgs::ImageConstPtr& depthMsg,
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg)
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
 {
 	std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
 	std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
@@ -678,14 +690,15 @@ void CoreWrapper::commonDepthCallback(
 	imageMsgs.push_back(imageMsg);
 	depthMsgs.push_back(depthMsg);
 	cameraInfoMsgs.push_back(cameraInfoMsg);
-	commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg);
+	commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg);
 }
 void CoreWrapper::commonDepthCallback(
 		const std::string & odomFrameId,
 		const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
 		const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
 		const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
-		const sensor_msgs::LaserScanConstPtr& scanMsg)
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
 {
 	UASSERT(imageMsgs.size()>0 &&
 			imageMsgs.size() == depthMsgs.size() &&
@@ -704,7 +717,7 @@ void CoreWrapper::commonDepthCallback(
 	int cameraCount = imageMsgs.size();
 	cv::Mat rgb;
 	cv::Mat depth;
-	pcl::PointCloud<pcl::PointXYZ> scanCloud;
+	pcl::PointCloud<pcl::PointXYZ> scanCloud2d;
 	std::vector<CameraModel> cameraModels;
 	int genMaxScanPts = 0;
 	for(unsigned int i=0; i<imageMsgs.size(); ++i)
@@ -813,9 +826,9 @@ void CoreWrapper::commonDepthCallback(
 				model.cy(),
 				localTransform));
 
-		if(scanMsg.get() == 0 && genScan_)
+		if(scan2dMsg.get() == 0 && genScan_)
 		{
-			scanCloud += util3d::laserScanFromDepthImage(
+			scanCloud2d += util3d::laserScanFromDepthImage(
 					subDepth,
 					model.fx(),
 					model.fy(),
@@ -828,10 +841,10 @@ void CoreWrapper::commonDepthCallback(
 	}
 
 	cv::Mat scan;
-	if(scanMsg.get() != 0)
+	if(scan2dMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
+		if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
 		{
 			return;
 		}
@@ -839,16 +852,16 @@ void CoreWrapper::commonDepthCallback(
 		//transform in frameId_ frame
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(scanOut, *pclScan);
 
 		// sync with odometry stamp
-		if(lastPoseStamp_ != scanMsg->header.stamp)
+		if(lastPoseStamp_ != scan2dMsg->header.stamp)
 		{
 			if(!odomT.isNull())
 			{
-				Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp);
+				Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp);
 				if(sensorT.isNull())
 				{
 					return;
@@ -858,19 +871,27 @@ void CoreWrapper::commonDepthCallback(
 
 			}
 		}
+		scan = util3d::laserScan2dFromPointCloud(*pclScan);
+	}
+	else if(scan3dMsg.get() != 0)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*scan3dMsg, *pclScan);
 		scan = util3d::laserScanFromPointCloud(*pclScan);
 	}
-	else if(scanCloud.size())
+	else if(scanCloud2d.size())
 	{
-		scan = util3d::laserScanFromPointCloud(scanCloud);
+		scan = util3d::laserScan2dFromPointCloud(scanCloud2d);
 	}
 
-	ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:depthMsgs[0]->header.stamp;
+	ros::Time stamp =   scan2dMsg.get() != 0?scan2dMsg->header.stamp:
+						scan3dMsg.get() != 0?scan3dMsg->header.stamp:
+					    depthMsgs[0]->header.stamp;
 
 	process(stamp,
 			SensorData(scan,
-					scanMsg.get() != 0?(int)scanMsg->ranges.size():genMaxScanPts,
-					scanMsg.get() != 0?scanMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
+					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():genMaxScanPts,
+					scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
 					rgb,
 					depth,
 					cameraModels,
@@ -890,7 +911,8 @@ void CoreWrapper::commonStereoCallback(
 		const sensor_msgs::ImageConstPtr& rightImageMsg,
 		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg)
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
 {
 	if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
 		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
@@ -934,10 +956,10 @@ void CoreWrapper::commonStereoCallback(
 	}
 
 	cv::Mat scan;
-	if(scanMsg.get() != 0)
+	if(scan2dMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
+		if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
 		{
 			return;
 		}
@@ -946,16 +968,16 @@ void CoreWrapper::commonStereoCallback(
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
 		//projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfBuffer_);
-		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(scanOut, *pclScan);
 
 		// sync with odometry stamp
-		if(lastPoseStamp_ != scanMsg->header.stamp)
+		if(lastPoseStamp_ != scan2dMsg->header.stamp)
 		{
 			if(!odomT.isNull())
 			{
-				Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp);
+				Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp);
 				if(sensorT.isNull())
 				{
 					return;
@@ -966,6 +988,12 @@ void CoreWrapper::commonStereoCallback(
 			}
 		}
 
+		scan = util3d::laserScan2dFromPointCloud(*pclScan);
+	}
+	else if(scan3dMsg.get() != 0)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*scan3dMsg, *pclScan);
 		scan = util3d::laserScanFromPointCloud(*pclScan);
 	}
 
@@ -1004,11 +1032,13 @@ void CoreWrapper::commonStereoCallback(
 		}
 	}
 
-	ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp;
+	ros::Time stamp =   scan2dMsg.get() != 0?scan2dMsg->header.stamp:
+						scan3dMsg.get() != 0?scan3dMsg->header.stamp:
+					    leftImageMsg->header.stamp;
 	process(stamp,
 			SensorData(scan,
-					scanMsg.get() != 0?(int)scanMsg->ranges.size():0,
-					scanMsg.get() != 0?scanMsg->range_max:0,
+					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():0,
+					scan2dMsg.get() != 0?scan2dMsg->range_max:0,
 					ptrLeftImage->image,
 					ptrRightImage->image,
 					stereoModel,
@@ -1035,7 +1065,8 @@ void CoreWrapper::depthCallback(
 	}
 
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg);
 }
 void CoreWrapper::depthScanCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -1048,7 +1079,22 @@ void CoreWrapper::depthScanCallback(
 	{
 		return;
 	}
-	commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg);
+}
+void CoreWrapper::depthScan3dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	if(!commonOdomUpdate(odomMsg))
+	{
+		return;
+	}
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scan2dMsg, scanMsg);
 }
 void CoreWrapper::stereoCallback(
 		const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -1063,7 +1109,8 @@ void CoreWrapper::stereoCallback(
 	}
 
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg);
 }
 void CoreWrapper::stereoScanCallback(
 		const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -1077,7 +1124,23 @@ void CoreWrapper::stereoScanCallback(
 	{
 		return;
 	}
-	commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg);
+}
+void CoreWrapper::stereoScan3dCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+	   const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+	   const nav_msgs::OdometryConstPtr & odomMsg)
+{
+	if(!commonOdomUpdate(odomMsg))
+	{
+		return;
+	}
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scanMsg);
 }
 
 void CoreWrapper::depth2Callback(
@@ -1105,7 +1168,8 @@ void CoreWrapper::depth2Callback(
 	cameraInfoMsgs.push_back(cameraInfo2Msg);
 
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg);
 }
 
 
@@ -1119,7 +1183,8 @@ void CoreWrapper::depthTFCallback(
 		return;
 	}
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg);
 }
 void CoreWrapper::depthScanTFCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -1131,7 +1196,21 @@ void CoreWrapper::depthScanTFCallback(
 	{
 		return;
 	}
-	commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg);
+}
+void CoreWrapper::depthScan3dTFCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	if(!commonOdomTFUpdate(scanMsg->header.stamp))
+	{
+		return;
+	}
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scan2dMsg, scanMsg);
 }
 void CoreWrapper::stereoTFCallback(
 		const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -1145,7 +1224,8 @@ void CoreWrapper::stereoTFCallback(
 	}
 
 	sensor_msgs::LaserScanConstPtr scanMsg; // null
-	commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg);
 }
 void CoreWrapper::stereoScanTFCallback(
 		const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -1158,7 +1238,23 @@ void CoreWrapper::stereoScanTFCallback(
 	{
 		return;
 	}
-	commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg);
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg);
+}
+
+void CoreWrapper::stereoScan3dTFCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+	   const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	if(!commonOdomTFUpdate(leftImageMsg->header.stamp))
+	{
+		return;
+	}
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // null
+	commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scanMsg);
 }
 
 void CoreWrapper::process(
@@ -2311,7 +2407,8 @@ bool CoreWrapper::octomapFullCallback(
  */
 void CoreWrapper::setupCallbacks(
 		bool subscribeDepth,
-		bool subscribeLaserScan,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
 		bool subscribeStereo,
 		int queueSize,
 		bool stereoApproxSync,
@@ -2323,7 +2420,7 @@ void CoreWrapper::setupCallbacks(
 	if(subscribeDepth)
 	{
 		UASSERT(depthCameras >= 1 && depthCameras <= 2);
-		UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!");
+		UASSERT_MSG(depthCameras == 1 || !(subscribeScan2d || subscribeScan3d || !odomFrameId_.empty()), "Not yet supported!");
 
 		imageSubs_.resize(depthCameras);
 		imageDepthSubs_.resize(depthCameras);
@@ -2357,7 +2454,7 @@ void CoreWrapper::setupCallbacks(
 		if(odomFrameId_.empty())
 		{
 			odomSub_.subscribe(nh, "odom", 1);
-			if(subscribeLaserScan)
+			if(subscribeScan2d)
 			{
 				ROS_INFO("Registering Depth+LaserScan callback...");
 				scanSub_.subscribe(nh, "scan", 1);
@@ -2378,6 +2475,27 @@ void CoreWrapper::setupCallbacks(
 						odomSub_.getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
+			else if(subscribeScan3d)
+			{
+				ROS_INFO("Registering Depth+LaserScan3d callback...");
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
+						MyDepthScan3dSyncPolicy(queueSize),
+						*imageSubs_[0],
+						odomSub_,
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0],
+						scan3dSub_);
+				depthScan3dSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
+
+				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+						ros::this_node::getName().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
+						odomSub_.getTopic().c_str(),
+						scan3dSub_.getTopic().c_str());
+			}
 			else //!subscribeLaserScan
 			{
 				if(depthCameras > 1)
@@ -2427,7 +2545,7 @@ void CoreWrapper::setupCallbacks(
 		else
 		{
 			// use odom from TF, so subscribe to sensors only
-			if(subscribeLaserScan)
+			if(subscribeScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
 				depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
@@ -2445,6 +2563,24 @@ void CoreWrapper::setupCallbacks(
 						cameraInfoSubs_[0]->getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
+			else if(subscribeScan3d)
+			{
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				depthScan3dTFSync_ = new message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy>(
+						MyDepthScan3dTFSyncPolicy(queueSize),
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0],
+						scan3dSub_);
+				depthScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4));
+
+				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+						ros::this_node::getName().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
+						scan3dSub_.getTopic().c_str());
+			}
 			else //!subscribeLaserScan
 			{
 				depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
@@ -2481,7 +2617,7 @@ void CoreWrapper::setupCallbacks(
 		if(odomFrameId_.empty())
 		{
 			odomSub_.subscribe(nh, "odom", 1);
-			if(subscribeLaserScan)
+			if(subscribeScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
 				stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
@@ -2503,6 +2639,28 @@ void CoreWrapper::setupCallbacks(
 						odomSub_.getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
+			else if(subscribeScan3d)
+			{
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				stereoScan3dSync_ = new message_filters::Synchronizer<MyStereoScan3dSyncPolicy>(
+						MyStereoScan3dSyncPolicy(queueSize),
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_,
+						scan3dSub_,
+						odomSub_);
+				stereoScan3dSync_->registerCallback(boost::bind(&CoreWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6));
+
+				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+						ros::this_node::getName().c_str(),
+						imageRectLeft_.getTopic().c_str(),
+						imageRectRight_.getTopic().c_str(),
+						cameraInfoLeft_.getTopic().c_str(),
+						cameraInfoRight_.getTopic().c_str(),
+						odomSub_.getTopic().c_str(),
+						scan3dSub_.getTopic().c_str());
+			}
 			else //!subscribeLaserScan
 			{
 				if(stereoApproxSync)
@@ -2542,9 +2700,9 @@ void CoreWrapper::setupCallbacks(
 		else
 		{
 			// use odom from TF, so subscribe to sensors only
-			if(subscribeLaserScan)
+			if(subscribeScan2d)
 			{
-				ROS_INFO("Registering Stereo+LaserScan+OdomTF callback...");
+				ROS_INFO("Registering Stereo+LaserScan2d+OdomTF callback...");
 				scanSub_.subscribe(nh, "scan", 1);
 				stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
 						MyStereoScanTFSyncPolicy(queueSize),
@@ -2563,6 +2721,27 @@ void CoreWrapper::setupCallbacks(
 						cameraInfoRight_.getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
+			else if(subscribeScan3d)
+			{
+				ROS_INFO("Registering Stereo+LaserScan3d+OdomTF callback...");
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
+						MyStereoScan3dTFSyncPolicy(queueSize),
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_,
+						scan3dSub_);
+				stereoScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5));
+
+				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+						ros::this_node::getName().c_str(),
+						imageRectLeft_.getTopic().c_str(),
+						imageRectRight_.getTopic().c_str(),
+						cameraInfoLeft_.getTopic().c_str(),
+						cameraInfoRight_.getTopic().c_str(),
+						scan3dSub_.getTopic().c_str());
+			}
 			else //!subscribeLaserScan
 			{
 				if(stereoApproxSync)

src/CoreWrapper.h

@@ -86,7 +86,8 @@ public:
 private:
 	void setupCallbacks(
 			bool subscribeDepth,
-			bool subscribeLaserScan,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
 			bool subscribeStereo,
 			int queueSize,
 			bool stereoApproxSync,
@@ -102,20 +103,23 @@ private:
 				const sensor_msgs::ImageConstPtr& imageMsg,
 				const sensor_msgs::ImageConstPtr& depthMsg,
 				const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg);
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
 	void commonDepthCallback(
 				const std::string & odomFrameId,
 				const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
 				const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
 				const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
-				const sensor_msgs::LaserScanConstPtr& scanMsg);
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
 	void commonStereoCallback(
 				const std::string & odomFrameId,
 				const sensor_msgs::ImageConstPtr& leftImageMsg,
 				const sensor_msgs::ImageConstPtr& rightImageMsg,
 				const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 				const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg);
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
 
 	// with odom msg
 	void depthCallback(
@@ -129,6 +133,12 @@ private:
 			const sensor_msgs::ImageConstPtr& imageDepthMsg,
 			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void depthScan3dCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageDepthMsg,
+			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
 	void stereoCallback(
 			const sensor_msgs::ImageConstPtr& leftImageMsg,
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
@@ -142,6 +152,13 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg,
 			const nav_msgs::OdometryConstPtr & odomMsg);
+	void stereoScan3dCallback(
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg);
 	void depth2Callback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
 			const sensor_msgs::ImageConstPtr& image1Msg,
@@ -161,6 +178,11 @@ private:
 			const sensor_msgs::ImageConstPtr& imageDepthMsg,
 			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void depthScan3dTFCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& imageDepthMsg,
+			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
 	void stereoTFCallback(
 			const sensor_msgs::ImageConstPtr& leftImageMsg,
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
@@ -172,6 +194,12 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void stereoScan3dTFCallback(
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
 
 	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
@@ -280,6 +308,7 @@ private:
 
 	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
@@ -289,6 +318,14 @@ private:
 			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy;
+	message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
 			nav_msgs::Odometry,
@@ -305,6 +342,15 @@ private:
 			nav_msgs::Odometry> MyStereoScanSyncPolicy;
 	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::PointCloud2,
+			nav_msgs::Odometry> MyStereoScan3dSyncPolicy;
+	message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
 			sensor_msgs::Image,
@@ -339,6 +385,13 @@ private:
 			sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy;
+	message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
 			sensor_msgs::Image,
@@ -353,6 +406,14 @@ private:
 			sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy;
 	message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::PointCloud2> MyStereoScan3dTFSyncPolicy;
+	message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
 			sensor_msgs::Image,

src/GuiWrapper.cpp

@@ -119,7 +119,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	ros::NodeHandle pnh("~");
 
 	// To receive odometry events
-	bool subscribeLaserScan = false;
+	bool subscribeLaserScan2d = false;
+	bool subscribeLaserScan3d = false;
 	bool subscribeDepth = false;
 	bool subscribeOdomInfo = false;
 	bool subscribeStereo = false;
@@ -130,7 +131,12 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
 	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
-	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+	if(pnh.getParam("subscribe_laserScan", subscribeLaserScan2d))
+	{
+		ROS_WARN("rtabmapviz: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
+	}
+	pnh.param("subscribe_scan", subscribeLaserScan2d, subscribeLaserScan2d);
+	pnh.param("subscribe_scan_cloud", subscribeLaserScan3d, subscribeLaserScan3d);
 	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
 	pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
 	pnh.param("depth_cameras", depthCameras, depthCameras);
@@ -181,7 +187,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 
 	this->setupCallbacks(
 			subscribeDepth,
-			subscribeLaserScan,
+			subscribeLaserScan2d,
+			subscribeLaserScan3d,
 			subscribeOdomInfo,
 			subscribeStereo,
 			queueSize,
@@ -510,7 +517,8 @@ void GuiWrapper::commonDepthCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
 		const sensor_msgs::ImageConstPtr& depthMsg,
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
@@ -519,7 +527,7 @@ void GuiWrapper::commonDepthCallback(
 	imageMsgs.push_back(imageMsg);
 	depthMsgs.push_back(depthMsg);
 	cameraInfoMsgs.push_back(cameraInfoMsg);
-	commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 
 void GuiWrapper::commonDepthCallback(
@@ -527,7 +535,8 @@ void GuiWrapper::commonDepthCallback(
 		const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
 		const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
 		const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
@@ -547,9 +556,13 @@ void GuiWrapper::commonDepthCallback(
 		}
 		else
 		{
-			if(scanMsg.get())
+			if(scan2dMsg.get())
 			{
-				odomHeader = scanMsg->header;
+				odomHeader = scan2dMsg->header;
+			}
+			else if(scan3dMsg.get())
+			{
+				odomHeader = scan3dMsg->header;
 			}
 			else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get())
 			{
@@ -691,10 +704,10 @@ void GuiWrapper::commonDepthCallback(
 		}
 
 		cv::Mat scan;
-		if(scanMsg.get() != 0)
+		if(scan2dMsg.get() != 0)
 		{
 			// make sure the frame of the laser is updated too
-			if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
+			if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
 			{
 				return;
 			}
@@ -702,16 +715,16 @@ void GuiWrapper::commonDepthCallback(
 			//transform in frameId_ frame
 			sensor_msgs::PointCloud2 scanOut;
 			laser_geometry::LaserProjection projection;
-			projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+			projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(scanOut, *pclScan);
 
 			// sync with odometry stamp
-			if(odomHeader.stamp != scanMsg->header.stamp)
+			if(odomHeader.stamp != scan2dMsg->header.stamp)
 			{
 				if(!odomT.isNull())
 				{
-					Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scanMsg->header.stamp);
+					Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp);
 					if(sensorT.isNull())
 					{
 						return;
@@ -723,6 +736,12 @@ void GuiWrapper::commonDepthCallback(
 			}
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
+		else if(scan3dMsg.get() != 0)
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			scan = util3d::laserScanFromPointCloud(*pclScan);
+		}
 
 		rtabmap::OdometryInfo info;
 		if(odomInfoMsg.get())
@@ -733,8 +752,8 @@ void GuiWrapper::commonDepthCallback(
 		rtabmap::OdometryEvent odomEvent(
 			rtabmap::SensorData(
 					scan,
-					scanMsg.get()?(int)scanMsg->ranges.size():0,
-					scanMsg.get()?(int)scanMsg->range_max:0,
+					scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
+					scan2dMsg.get()?(int)scan2dMsg->range_max:0,
 					rgb,
 					depth,
 					cameraModels,
@@ -754,7 +773,8 @@ void GuiWrapper::commonStereoCallback(
 		const sensor_msgs::ImageConstPtr& rightImageMsg,
 		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	// limit 10 Hz max
@@ -787,9 +807,13 @@ void GuiWrapper::commonStereoCallback(
 		}
 		else
 		{
-			if(scanMsg.get())
+			if(scan2dMsg.get())
 			{
-				odomHeader = scanMsg->header;
+				odomHeader = scan2dMsg->header;
+			}
+			else if(scan3dMsg.get())
+			{
+				odomHeader = scan3dMsg->header;
 			}
 			else
 			{
@@ -878,10 +902,10 @@ void GuiWrapper::commonStereoCallback(
 		cv::Mat right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
 
 		cv::Mat scan;
-		if(scanMsg.get() != 0)
+		if(scan2dMsg.get() != 0)
 		{
 			// make sure the frame of the laser is updated too
-			if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
+			if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
 			{
 				return;
 			}
@@ -889,16 +913,16 @@ void GuiWrapper::commonStereoCallback(
 			//transform in frameId_ frame
 			sensor_msgs::PointCloud2 scanOut;
 			laser_geometry::LaserProjection projection;
-			projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+			projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(scanOut, *pclScan);
 
 			// sync with odometry stamp
-			if(odomHeader.stamp != scanMsg->header.stamp)
+			if(odomHeader.stamp != scan2dMsg->header.stamp)
 			{
 				if(!odomT.isNull())
 				{
-					Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scanMsg->header.stamp);
+					Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp);
 					if(sensorT.isNull())
 					{
 						return;
@@ -908,6 +932,12 @@ void GuiWrapper::commonStereoCallback(
 
 				}
 			}
+			scan = util3d::laserScan2dFromPointCloud(*pclScan);
+		}
+		else if(scan3dMsg.get() != 0)
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::fromROSMsg(*scan3dMsg, *pclScan);
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
 
@@ -920,8 +950,8 @@ void GuiWrapper::commonStereoCallback(
 		rtabmap::OdometryEvent odomEvent(
 			rtabmap::SensorData(
 					scan,
-					scanMsg.get()?(int)scanMsg->ranges.size():0,
-					scanMsg.get()?(int)scanMsg->range_max:0,
+					scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
+					scan2dMsg.get()?(int)scan2dMsg->range_max:0,
 					left,
 					right,
 					stereoModel,
@@ -944,6 +974,7 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
 			sensor_msgs::ImageConstPtr(),
 			sensor_msgs::CameraInfoConstPtr(),
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -959,6 +990,7 @@ void GuiWrapper::depthCallback(
 			depthMsg,
 			cameraInfoMsg,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -987,6 +1019,7 @@ void GuiWrapper::depth2Callback(
 			depthMsgs,
 			cameraInfoMsgs,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -1003,6 +1036,7 @@ void GuiWrapper::depthOdomInfoCallback(
 			depthMsg,
 			cameraInfoMsg,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			odomInfoMsg);
 }
 
@@ -1032,6 +1066,7 @@ void GuiWrapper::depthOdomInfo2Callback(
 			depthMsgs,
 			cameraInfoMsgs,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			odomInfoMsg);
 }
 
@@ -1048,6 +1083,24 @@ void GuiWrapper::depthScanCallback(
 			depthMsg,
 			cameraInfoMsg,
 			scanMsg,
+			sensor_msgs::PointCloud2ConstPtr(),
+			rtabmap_ros::OdomInfoConstPtr());
+}
+
+void GuiWrapper::depthScan3dCallback(
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	commonDepthCallback(
+			odomMsg,
+			imageMsg,
+			depthMsg,
+			cameraInfoMsg,
+			sensor_msgs::LaserScanConstPtr(),
+			scanMsg,
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -1066,6 +1119,26 @@ void GuiWrapper::stereoScanCallback(
 			leftCameraInfoMsg,
 			rightCameraInfoMsg,
 			scanMsg,
+			sensor_msgs::PointCloud2ConstPtr(),
+			rtabmap_ros::OdomInfoConstPtr());
+}
+
+void GuiWrapper::stereoScan3dCallback(
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
+{
+	commonStereoCallback(
+			odomMsg,
+			leftImageMsg,
+			rightImageMsg,
+			leftCameraInfoMsg,
+			rightCameraInfoMsg,
+			sensor_msgs::LaserScanConstPtr(),
+			scanMsg,
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -1084,6 +1157,7 @@ void GuiWrapper::stereoOdomInfoCallback(
 			leftCameraInfoMsg,
 			rightCameraInfoMsg,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			odomInfoMsg);
 }
 
@@ -1101,6 +1175,7 @@ void GuiWrapper::stereoCallback(
 			leftCameraInfoMsg,
 			rightCameraInfoMsg,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -1116,6 +1191,7 @@ void GuiWrapper::depthTFCallback(
 			depthMsg,
 			cameraInfoMsg,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -1131,6 +1207,7 @@ void GuiWrapper::depthOdomInfoTFCallback(
 			depthMsg,
 			cameraInfoMsg,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			odomInfoMsg);
 }
 
@@ -1146,6 +1223,23 @@ void GuiWrapper::depthScanTFCallback(
 			depthMsg,
 			cameraInfoMsg,
 			scanMsg,
+			sensor_msgs::PointCloud2ConstPtr(),
+			rtabmap_ros::OdomInfoConstPtr());
+}
+
+void GuiWrapper::depthScan3dTFCallback(
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	commonDepthCallback(
+			nav_msgs::OdometryConstPtr(),
+			imageMsg,
+			depthMsg,
+			cameraInfoMsg,
+			sensor_msgs::LaserScanConstPtr(),
+			scanMsg,
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -1163,6 +1257,25 @@ void GuiWrapper::stereoScanTFCallback(
 			leftCameraInfoMsg,
 			rightCameraInfoMsg,
 			scanMsg,
+			sensor_msgs::PointCloud2ConstPtr(),
+			rtabmap_ros::OdomInfoConstPtr());
+}
+
+void GuiWrapper::stereoScan3dTFCallback(
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
+{
+	commonStereoCallback(
+			nav_msgs::OdometryConstPtr(),
+			leftImageMsg,
+			rightImageMsg,
+			leftCameraInfoMsg,
+			rightCameraInfoMsg,
+			sensor_msgs::LaserScanConstPtr(),
+			scanMsg,
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
@@ -1180,6 +1293,7 @@ void GuiWrapper::stereoOdomInfoTFCallback(
 			leftCameraInfoMsg,
 			rightCameraInfoMsg,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			odomInfoMsg);
 }
 
@@ -1196,12 +1310,14 @@ void GuiWrapper::stereoTFCallback(
 			leftCameraInfoMsg,
 			rightCameraInfoMsg,
 			sensor_msgs::LaserScanConstPtr(),
+			sensor_msgs::PointCloud2ConstPtr(),
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
 void GuiWrapper::setupCallbacks(
 		bool subscribeDepth,
-		bool subscribeLaserScan,
+		bool subscribeLaserScan2d,
+		bool subscribeLaserScan3d,
 		bool subscribeOdomInfo,
 		bool subscribeStereo,
 		int queueSize,
@@ -1216,7 +1332,7 @@ void GuiWrapper::setupCallbacks(
 				 "same time. Parameter subscribe_depth is set to false.");
 		subscribeDepth = false;
 	}
-	if(!subscribeDepth && !subscribeStereo && subscribeLaserScan)
+	if(!subscribeDepth && !subscribeStereo && (subscribeLaserScan2d || subscribeLaserScan3d))
 	{
 		ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription...");
 	}
@@ -1233,7 +1349,7 @@ void GuiWrapper::setupCallbacks(
 	if(subscribeDepth)
 	{
 		UASSERT(depthCameras >= 1 && depthCameras <= 2);
-		UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!");
+		UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan2d || subscribeLaserScan3d || !odomFrameId_.empty()), "Not yet supported!");
 
 		imageSubs_.resize(depthCameras);
 		imageDepthSubs_.resize(depthCameras);
@@ -1267,7 +1383,7 @@ void GuiWrapper::setupCallbacks(
 		if(odomFrameId_.empty())
 		{
 			odomSub_.subscribe(nh, "odom", 1);
-			if(subscribeLaserScan)
+			if(subscribeLaserScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
 				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
@@ -1287,6 +1403,26 @@ void GuiWrapper::setupCallbacks(
 						odomSub_.getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
+			else if(subscribeLaserScan3d)
+			{
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
+						MyDepthScan3dSyncPolicy(queueSize),
+						scan3dSub_,
+						odomSub_,
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0]);
+				depthScan3dSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
+
+				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+						ros::this_node::getName().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
+						odomSub_.getTopic().c_str(),
+						scan3dSub_.getTopic().c_str());
+			}
 			else if(subscribeOdomInfo)
 			{
 				odomInfoSub_.subscribe(nh, "odom_info", 1);
@@ -1382,7 +1518,7 @@ void GuiWrapper::setupCallbacks(
 		else
 		{
 			// use TF as odom
-			if(subscribeLaserScan)
+			if(subscribeLaserScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
 				depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
@@ -1400,6 +1536,24 @@ void GuiWrapper::setupCallbacks(
 						cameraInfoSubs_[0]->getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
+			else if(subscribeLaserScan3d)
+			{
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				depthScan3dTFSync_ = new message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy>(
+						MyDepthScan3dTFSyncPolicy(queueSize),
+						scan3dSub_,
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0]);
+				depthScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4));
+
+				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+						ros::this_node::getName().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
+						scan3dSub_.getTopic().c_str());
+			}
 			else if(subscribeOdomInfo)
 			{
 				odomInfoSub_.subscribe(nh, "odom_info", 1);
@@ -1454,7 +1608,7 @@ void GuiWrapper::setupCallbacks(
 		if(odomFrameId_.empty())
 		{
 			odomSub_.subscribe(nh, "odom", 1);
-			if(subscribeLaserScan)
+			if(subscribeLaserScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
 				stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
@@ -1476,6 +1630,28 @@ void GuiWrapper::setupCallbacks(
 						odomSub_.getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
+			else if(subscribeLaserScan3d)
+			{
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				stereoScan3dSync_ = new message_filters::Synchronizer<MyStereoScan3dSyncPolicy>(
+						MyStereoScan3dSyncPolicy(queueSize),
+						scan3dSub_,
+						odomSub_,
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_);
+				stereoScan3dSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6));
+
+				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+						ros::this_node::getName().c_str(),
+						imageRectLeft_.getTopic().c_str(),
+						imageRectRight_.getTopic().c_str(),
+						cameraInfoLeft_.getTopic().c_str(),
+						cameraInfoRight_.getTopic().c_str(),
+						odomSub_.getTopic().c_str(),
+						scan3dSub_.getTopic().c_str());
+			}
 			else if(subscribeOdomInfo)
 			{
 				odomInfoSub_.subscribe(nh, "odom_info", 1);
@@ -1521,7 +1697,7 @@ void GuiWrapper::setupCallbacks(
 		else
 		{
 			//use odom TF
-			if(subscribeLaserScan)
+			if(subscribeLaserScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
 				stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
@@ -1541,6 +1717,26 @@ void GuiWrapper::setupCallbacks(
 						cameraInfoRight_.getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
+			else if(subscribeLaserScan3d)
+			{
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
+						MyStereoScan3dTFSyncPolicy(queueSize),
+						scan3dSub_,
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_);
+				stereoScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5));
+
+				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+						ros::this_node::getName().c_str(),
+						imageRectLeft_.getTopic().c_str(),
+						imageRectRight_.getTopic().c_str(),
+						cameraInfoLeft_.getTopic().c_str(),
+						cameraInfoRight_.getTopic().c_str(),
+						scan3dSub_.getTopic().c_str());
+			}
 			else if(subscribeOdomInfo)
 			{
 				odomInfoSub_.subscribe(nh, "odom_info", 1);

src/GuiWrapper.h

@@ -80,7 +80,8 @@ private:
 
 	void setupCallbacks(
 			bool subscribeDepth,
-			bool subscribeLaserScan,
+			bool subscribeLaserScan2d,
+			bool subscribeLaserScan3d,
 			bool subscribeOdomInfo,
 			bool subscribeStereo,
 			int queueSize,
@@ -91,14 +92,16 @@ private:
 			const sensor_msgs::ImageConstPtr& imageMsg,
 			const sensor_msgs::ImageConstPtr& depthMsg,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
 	void commonDepthCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
 			const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
 			const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
 			const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
 	void commonStereoCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
@@ -106,7 +109,8 @@ private:
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
 
 	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
@@ -146,6 +150,12 @@ private:
 			const sensor_msgs::ImageConstPtr& imageMsg,
 		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
 		    const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void depthScan3dCallback(
+			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& imageDepthMsg,
+			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
 
 	void stereoScanCallback(
 			const sensor_msgs::LaserScanConstPtr& scanMsg,
@@ -154,6 +164,13 @@ private:
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+	void stereoScan3dCallback(
+			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
 	void stereoOdomInfoCallback(
 			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
 			const nav_msgs::OdometryConstPtr & odomMsg,
@@ -182,6 +199,11 @@ private:
 			const sensor_msgs::ImageConstPtr& imageMsg,
 		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
 		    const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
+	void depthScan3dTFCallback(
+			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& imageDepthMsg,
+			const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
 
 	void stereoScanTFCallback(
 			const sensor_msgs::LaserScanConstPtr& scanMsg,
@@ -189,6 +211,12 @@ private:
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+	void stereoScan3dTFCallback(
+			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
 	void stereoOdomInfoTFCallback(
 			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
 			const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -231,6 +259,7 @@ private:
 	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
 	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
 
 	image_transport::SubscriberFilter imageRectLeft_;
 	image_transport::SubscriberFilter imageRectRight_;
@@ -256,6 +285,14 @@ private:
 			sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::PointCloud2,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy;
+	message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			nav_msgs::Odometry,
 			sensor_msgs::Image,
@@ -288,6 +325,15 @@ private:
 			sensor_msgs::CameraInfo> MyStereoScanSyncPolicy;
 	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::PointCloud2,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy;
+	message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			rtabmap_ros::OdomInfo,
 			nav_msgs::Odometry,
@@ -326,6 +372,13 @@ private:
 			sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::PointCloud2,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthScan3dTFSyncPolicy;
+	message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
 			sensor_msgs::Image,
@@ -354,6 +407,14 @@ private:
 			sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
 	message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::PointCloud2,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoScan3dTFSyncPolicy;
+	message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			rtabmap_ros::OdomInfo,
 			sensor_msgs::Image,

src/MapsManager.cpp

@@ -66,6 +66,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 	pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_);
 
 	// scan map stuff
+	pnh.param("scan_decimation", scanDecimation_, scanDecimation_);
 	pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
 	pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_);
 
@@ -344,21 +345,28 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						{
 							if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0))
 							{
-								pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
-								if(scanVoxelSize_ > 0.0)
+								if(scanDecimation_ > 1)
 								{
-									scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_);
-									if(gridRequired)
+									scan = util3d::downsample(scan, scanDecimation_);
+								}
+								if(scanRequired || scanVoxelSize_ > 0.0)
+								{
+									pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
+									if(scanVoxelSize_ > 0.0)
 									{
-										scan = util3d::laserScanFromPointCloud(*scanCloud);
+										scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_);
+										if(gridRequired && scan.type() == CV_32FC2)
+										{
+											scan = util3d::laserScan2dFromPointCloud(*scanCloud);
+										}
+									}
+									if(scanRequired)
+									{
+										uInsert(scans_, std::make_pair(iter->first, scanCloud));
 									}
 								}
-								if(scanRequired)
-								{
-									uInsert(scans_, std::make_pair(iter->first, scanCloud));
-								}
 							}
-							if(gridRequired)
+							if(gridRequired && scan.type() == CV_32FC2)
 							{
 								cv::Mat ground, obstacles;
 								util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());

src/MapsManager.h

@@ -74,6 +74,7 @@ private:
 	bool cloudFrustumCulling_;
 	double cloudNoiseFilteringRadius_;
 	int cloudNoiseFilteringMinNeighbors_;
+	int scanDecimation_;
 	double scanVoxelSize_;
 	bool scanOutputVoxelized_;
 	double projMaxGroundAngle_;