Fixed issue #47

src/GuiWrapper.cpp

@@ -1069,6 +1069,24 @@ void GuiWrapper::depthScanCallback(
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
+void GuiWrapper::depthScanOdomInfoCallback(
+		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& 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,
+			scanMsg,
+			sensor_msgs::PointCloud2ConstPtr(),
+			odomInfoMsg);
+}
+
 void GuiWrapper::depthScan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -1086,6 +1104,24 @@ void GuiWrapper::depthScan3dCallback(
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
+void GuiWrapper::depthScan3dOdomInfoCallback(
+		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+		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,
+			odomInfoMsg);
+}
+
 void GuiWrapper::stereoScanCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -1105,6 +1141,26 @@ void GuiWrapper::stereoScanCallback(
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
+void GuiWrapper::stereoScanOdomInfoCallback(
+		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& 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,
+			scanMsg,
+			sensor_msgs::PointCloud2ConstPtr(),
+			odomInfoMsg);
+}
+
 void GuiWrapper::stereoScan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -1124,6 +1180,26 @@ void GuiWrapper::stereoScan3dCallback(
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
+void GuiWrapper::stereoScan3dOdomInfoCallback(
+		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+		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,
+			odomInfoMsg);
+}
+
 void GuiWrapper::stereoOdomInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -1368,42 +1444,92 @@ void GuiWrapper::setupCallbacks(
 			if(subscribeLaserScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
+				if(subscribeOdomInfo)
-						MyDepthScanSyncPolicy(queueSize),
+				{
-						scanSub_,
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
-						odomSub_,
+					depthScanOdomInfoSync_ = new message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy>(
-						*imageSubs_[0],
+							MyDepthScanOdomInfoSyncPolicy(queueSize),
-						*imageDepthSubs_[0],
+							odomInfoSub_,
-						*cameraInfoSubs_[0]);
+							scanSub_,
-				depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+							odomSub_,
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
 
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
-						ros::this_node::getName().c_str(),
+							ros::this_node::getName().c_str(),
-						imageSubs_[0]->getTopic().c_str(),
+							imageSubs_[0]->getTopic().c_str(),
-						imageDepthSubs_[0]->getTopic().c_str(),
+							imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSubs_[0]->getTopic().c_str(),
+							cameraInfoSubs_[0]->getTopic().c_str(),
-						odomSub_.getTopic().c_str(),
+							odomSub_.getTopic().c_str(),
-						scanSub_.getTopic().c_str());
+							scanSub_.getTopic().c_str(),
+							odomInfoSub_.getTopic().c_str());
+				}
+				else
+				{
+					depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
+							MyDepthScanSyncPolicy(queueSize),
+							scanSub_,
+							odomSub_,
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, 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(),
+							scanSub_.getTopic().c_str());
+				}
 			}
 			else if(subscribeLaserScan3d)
 			{
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
-				depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
+				if(subscribeOdomInfo)
-						MyDepthScan3dSyncPolicy(queueSize),
+				{
-						scan3dSub_,
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
-						odomSub_,
+					depthScan3dOdomInfoSync_ = new message_filters::Synchronizer<MyDepthScan3dOdomInfoSyncPolicy>(
-						*imageSubs_[0],
+							MyDepthScan3dOdomInfoSyncPolicy(queueSize),
-						*imageDepthSubs_[0],
+							odomInfoSub_,
-						*cameraInfoSubs_[0]);
+							scan3dSub_,
-				depthScan3dSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
+							odomSub_,
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
 
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
-						ros::this_node::getName().c_str(),
+							ros::this_node::getName().c_str(),
-						imageSubs_[0]->getTopic().c_str(),
+							imageSubs_[0]->getTopic().c_str(),
-						imageDepthSubs_[0]->getTopic().c_str(),
+							imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSubs_[0]->getTopic().c_str(),
+							cameraInfoSubs_[0]->getTopic().c_str(),
-						odomSub_.getTopic().c_str(),
+							odomSub_.getTopic().c_str(),
-						scan3dSub_.getTopic().c_str());
+							scan3dSub_.getTopic().c_str(),
+							odomInfoSub_.getTopic().c_str());
+				}
+				else
+				{
+					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)
 			{
@@ -1593,46 +1719,100 @@ void GuiWrapper::setupCallbacks(
 			if(subscribeLaserScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
+				if(subscribeOdomInfo)
-						MyStereoScanSyncPolicy(queueSize),
+				{
-						scanSub_,
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
-						odomSub_,
+					stereoScanOdomInfoSync_ = new message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy>(
-						imageRectLeft_,
+							MyStereoScanOdomInfoSyncPolicy(queueSize),
-						imageRectRight_,
+							odomInfoSub_,
-						cameraInfoLeft_,
+							scanSub_,
-						cameraInfoRight_);
+							odomSub_,
-				stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
+							imageRectLeft_,
+							imageRectRight_,
+							cameraInfoLeft_,
+							cameraInfoRight_);
+					stereoScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7));
 
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
-						ros::this_node::getName().c_str(),
+							ros::this_node::getName().c_str(),
-						imageRectLeft_.getTopic().c_str(),
+							imageRectLeft_.getTopic().c_str(),
-						imageRectRight_.getTopic().c_str(),
+							imageRectRight_.getTopic().c_str(),
-						cameraInfoLeft_.getTopic().c_str(),
+							cameraInfoLeft_.getTopic().c_str(),
-						cameraInfoRight_.getTopic().c_str(),
+							cameraInfoRight_.getTopic().c_str(),
-						odomSub_.getTopic().c_str(),
+							odomSub_.getTopic().c_str(),
-						scanSub_.getTopic().c_str());
+							scanSub_.getTopic().c_str(),
+							odomInfoSub_.getTopic().c_str());
+				}
+				else
+				{
+					stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
+							MyStereoScanSyncPolicy(queueSize),
+							scanSub_,
+							odomSub_,
+							imageRectLeft_,
+							imageRectRight_,
+							cameraInfoLeft_,
+							cameraInfoRight_);
+					stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, 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(),
+							scanSub_.getTopic().c_str());
+				}
 			}
 			else if(subscribeLaserScan3d)
 			{
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
-				stereoScan3dSync_ = new message_filters::Synchronizer<MyStereoScan3dSyncPolicy>(
+				if(subscribeOdomInfo)
-						MyStereoScan3dSyncPolicy(queueSize),
+				{
-						scan3dSub_,
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
-						odomSub_,
+					stereoScan3dOdomInfoSync_ = new message_filters::Synchronizer<MyStereoScan3dOdomInfoSyncPolicy>(
-						imageRectLeft_,
+							MyStereoScan3dOdomInfoSyncPolicy(queueSize),
-						imageRectRight_,
+							odomInfoSub_,
-						cameraInfoLeft_,
+							scan3dSub_,
-						cameraInfoRight_);
+							odomSub_,
-				stereoScan3dSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6));
+							imageRectLeft_,
+							imageRectRight_,
+							cameraInfoLeft_,
+							cameraInfoRight_);
+					stereoScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7));
 
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
-						ros::this_node::getName().c_str(),
+							ros::this_node::getName().c_str(),
-						imageRectLeft_.getTopic().c_str(),
+							imageRectLeft_.getTopic().c_str(),
-						imageRectRight_.getTopic().c_str(),
+							imageRectRight_.getTopic().c_str(),
-						cameraInfoLeft_.getTopic().c_str(),
+							cameraInfoLeft_.getTopic().c_str(),
-						cameraInfoRight_.getTopic().c_str(),
+							cameraInfoRight_.getTopic().c_str(),
-						odomSub_.getTopic().c_str(),
+							odomSub_.getTopic().c_str(),
-						scan3dSub_.getTopic().c_str());
+							scan3dSub_.getTopic().c_str(),
+							odomInfoSub_.getTopic().c_str());
+				}
+				else
+				{
+					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)
 			{

src/GuiWrapper.h

@@ -150,12 +150,26 @@ private:
 			const sensor_msgs::ImageConstPtr& imageMsg,
 		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
 		    const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void depthScanOdomInfoCallback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			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 depthScan3dOdomInfoCallback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			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,
@@ -164,6 +178,14 @@ private:
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+	void stereoScanOdomInfoCallback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const sensor_msgs::LaserScanConstPtr& 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 stereoScan3dCallback(
 			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 			const nav_msgs::OdometryConstPtr & odomMsg,
@@ -171,6 +193,14 @@ private:
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+	void stereoScan3dOdomInfoCallback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			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,
@@ -285,6 +315,15 @@ private:
 			sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::LaserScan,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthScanOdomInfoSyncPolicy;
+	message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy> * depthScanOdomInfoSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::PointCloud2,
 			nav_msgs::Odometry,
@@ -293,6 +332,15 @@ private:
 			sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy;
 	message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::PointCloud2,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthScan3dOdomInfoSyncPolicy;
+	message_filters::Synchronizer<MyDepthScan3dOdomInfoSyncPolicy> * depthScan3dOdomInfoSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			nav_msgs::Odometry,
 			sensor_msgs::Image,
@@ -325,6 +373,16 @@ private:
 			sensor_msgs::CameraInfo> MyStereoScanSyncPolicy;
 	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::LaserScan,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoScanOdomInfoSyncPolicy;
+	message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy> * stereoScanOdomInfoSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::PointCloud2,
 			nav_msgs::Odometry,
@@ -334,6 +392,16 @@ private:
 			sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy;
 	message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::PointCloud2,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoScan3dOdomInfoSyncPolicy;
+	message_filters::Synchronizer<MyStereoScan3dOdomInfoSyncPolicy> * stereoScan3dOdomInfoSync_;
+
 	typedef message_filters::sync_policies::ApproximateTime<
 			rtabmap_ros::OdomInfo,
 			nav_msgs::Odometry,