Merge branch 'master' of https://github.com/introlab/rtabmap_ros

include/rtabmap_ros/CoreWrapper.h

@@ -323,6 +323,13 @@ private:
 			sensor_msgs::CameraInfo,
 			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::LaserScan> MyDepthScanExactSyncPolicy;
+	message_filters::Synchronizer<MyDepthScanExactSyncPolicy> * depthScanExactSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
@@ -331,6 +338,13 @@ private:
 			sensor_msgs::CameraInfo,
 			sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy;
 	message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::PointCloud2> MyDepthScan3dExactSyncPolicy;
+	message_filters::Synchronizer<MyDepthScan3dExactSyncPolicy> * depthScan3dExactSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
@@ -396,6 +410,12 @@ private:
 			sensor_msgs::CameraInfo,
 			sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::LaserScan> MyDepthScanTFExactSyncPolicy;
+	message_filters::Synchronizer<MyDepthScanTFExactSyncPolicy> * depthScanTFExactSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
@@ -403,6 +423,12 @@ private:
 			sensor_msgs::CameraInfo,
 			sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy;
 	message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::PointCloud2> MyDepthScan3dTFExactSyncPolicy;
+	message_filters::Synchronizer<MyDepthScan3dTFExactSyncPolicy> * depthScan3dTFExactSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,

src/CoreWrapper.cpp

@@ -2708,17 +2708,31 @@ void CoreWrapper::setupCallbacks(
 			if(subscribeScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
+				if(approxSync)
-						MyDepthScanSyncPolicy(queueSize),
+				{
-						*imageSubs_[0],
+					depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
-						odomSub_,
+							MyDepthScanSyncPolicy(queueSize),
-						*imageDepthSubs_[0],
+							*imageSubs_[0],
-						*cameraInfoSubs_[0],
+							odomSub_,
-						scanSub_);
+							*imageDepthSubs_[0],
-				depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+							*cameraInfoSubs_[0],
-
+							scanSub_);
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+				}
+				else
+				{
+					depthScanExactSync_ = new message_filters::Synchronizer<MyDepthScanExactSyncPolicy>(
+							MyDepthScanExactSyncPolicy(queueSize),
+							*imageSubs_[0],
+							odomSub_,
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0],
+							scanSub_);
+					depthScanExactSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+				}
+				ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
+						approxSync?"approx":"exact",
 						imageSubs_[0]->getTopic().c_str(),
 						imageDepthSubs_[0]->getTopic().c_str(),
 						cameraInfoSubs_[0]->getTopic().c_str(),
@@ -2728,17 +2742,31 @@ void CoreWrapper::setupCallbacks(
 			else if(subscribeScan3d)
 			{
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
-				depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
+				if(approxSync)
-						MyDepthScan3dSyncPolicy(queueSize),
+				{
-						*imageSubs_[0],
+					depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
-						odomSub_,
+							MyDepthScan3dSyncPolicy(queueSize),
-						*imageDepthSubs_[0],
+							*imageSubs_[0],
-						*cameraInfoSubs_[0],
+							odomSub_,
-						scan3dSub_);
+							*imageDepthSubs_[0],
-				depthScan3dSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
+							*cameraInfoSubs_[0],
-
+							scan3dSub_);
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					depthScan3dSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
+				}
+				else
+				{
+					depthScan3dExactSync_ = new message_filters::Synchronizer<MyDepthScan3dExactSyncPolicy>(
+							MyDepthScan3dExactSyncPolicy(queueSize),
+							*imageSubs_[0],
+							odomSub_,
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0],
+							scan3dSub_);
+					depthScan3dExactSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
+				}
+				ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
+						approxSync?"approx":"exact",
 						imageSubs_[0]->getTopic().c_str(),
 						imageDepthSubs_[0]->getTopic().c_str(),
 						cameraInfoSubs_[0]->getTopic().c_str(),
@@ -2808,16 +2836,29 @@ void CoreWrapper::setupCallbacks(
 			if(subscribeScan2d)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
+				if(approxSync)
-						MyDepthScanTFSyncPolicy(queueSize),
+				{
-						*imageSubs_[0],
+					depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
-						*imageDepthSubs_[0],
+							MyDepthScanTFSyncPolicy(queueSize),
-						*cameraInfoSubs_[0],
+							*imageSubs_[0],
-						scanSub_);
+							*imageDepthSubs_[0],
-				depthScanTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
+							*cameraInfoSubs_[0],
-
+							scanSub_);
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+					depthScanTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
+				}
+				else
+				{
+					depthScanTFExactSync_ = new message_filters::Synchronizer<MyDepthScanTFExactSyncPolicy>(
+							MyDepthScanTFExactSyncPolicy(queueSize),
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0],
+							scanSub_);
+					depthScanTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
+				}
+				ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
+						approxSync?"approx":"exact",
 						imageSubs_[0]->getTopic().c_str(),
 						imageDepthSubs_[0]->getTopic().c_str(),
 						cameraInfoSubs_[0]->getTopic().c_str(),
@@ -2826,16 +2867,29 @@ void CoreWrapper::setupCallbacks(
 			else if(subscribeScan3d)
 			{
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
-				depthScan3dTFSync_ = new message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy>(
+				if(approxSync)
-						MyDepthScan3dTFSyncPolicy(queueSize),
+				{
-						*imageSubs_[0],
+					depthScan3dTFSync_ = new message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy>(
-						*imageDepthSubs_[0],
+							MyDepthScan3dTFSyncPolicy(queueSize),
-						*cameraInfoSubs_[0],
+							*imageSubs_[0],
-						scan3dSub_);
+							*imageDepthSubs_[0],
-				depthScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4));
+							*cameraInfoSubs_[0],
-
+							scan3dSub_);
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+					depthScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4));
+				}
+				else
+				{
+					depthScan3dTFExactSync_ = new message_filters::Synchronizer<MyDepthScan3dTFExactSyncPolicy>(
+							MyDepthScan3dTFExactSyncPolicy(queueSize),
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0],
+							scan3dSub_);
+					depthScan3dTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4));
+				}
+				ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
+						approxSync?"approx":"exact",
 						imageSubs_[0]->getTopic().c_str(),
 						imageDepthSubs_[0]->getTopic().c_str(),
 						cameraInfoSubs_[0]->getTopic().c_str(),