Added approx_sync_max_interval parameter to odometry and sync nodes.

launch/rtabmap.launch

@@ -57,7 +57,8 @@
 
   <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
   <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
-  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>         
+  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>     
+  <arg name="approx_sync_max_interval"  default="0"/> <!-- (sec) 0 means infinite interval duration (used with approx_sync=true) -->
    
   <!-- RGB-D related topics -->
   <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
@@ -174,6 +175,7 @@
           <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
           <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
           <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
+          <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
           <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
           <param name="depth_scale"     type="double" value="$(arg rgbd_depth_scale)"/>
           <param name="decimation"     type="double" value="$(arg rgbd_decimation)"/>
@@ -195,6 +197,7 @@
           <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
           <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
           <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
+          <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
           <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
         </node>
       </group>
@@ -219,6 +222,7 @@
       <param name="decimation"  type="double" value="$(arg gen_cloud_decimation)"/>
       <param name="voxel_size"  type="double" value="$(arg gen_cloud_voxel)"/>
       <param name="approx_sync" type="bool"   value="false"/>
+      <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
     </node>
 
     <!-- Visual odometry -->
@@ -242,6 +246,7 @@
           <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
           <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
           <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
+          <param name="approx_sync_max_interval"    type="double" value="$(arg approx_sync_max_interval)"/>
           <param name="config_path"                 type="string" value="$(arg cfg)"/>
           <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
           <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
@@ -271,6 +276,7 @@
           <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
           <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
           <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
+          <param name="approx_sync_max_interval"    type="double" value="$(arg approx_sync_max_interval)"/>
           <param name="config_path"                 type="string" value="$(arg cfg)"/>
           <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
           <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
@@ -450,6 +456,7 @@
     <param name="decimation"  type="double" value="4"/>
     <param name="voxel_size"  type="double" value="0.0"/>
     <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
+    <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
   </node>
 
 </launch>

src/nodelets/data_throttle.cpp

@@ -89,6 +89,7 @@ private:
 
 		int queueSize = 10;
 		bool approxSync = true;
+		double approxSyncMaxInterval = 0.0;
 		if(private_nh.getParam("max_rate", rate_))
 		{
 			NODELET_WARN("\"max_rate\" is now known as \"rate\".");
@@ -96,15 +97,20 @@ private:
 		private_nh.param("rate", rate_, rate_);
 		private_nh.param("queue_size", queueSize, queueSize);
 		private_nh.param("approx_sync", approxSync, approxSync);
+		private_nh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		private_nh.param("decimation", decimation_, decimation_);
 		ROS_ASSERT(decimation_ >= 1);
-		NODELET_INFO("Rate=%f Hz", rate_);
+		NODELET_INFO("rate=%f Hz", rate_);
-		NODELET_INFO("Decimation=%d", decimation_);
+		NODELET_INFO("decimation=%d", decimation_);
-		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
+		NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
 
 		if(approxSync)
 		{
 			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
 		}
 		else

src/nodelets/point_cloud_aggregator.cpp

@@ -102,10 +102,12 @@ private:
 		int queueSize = 5;
 		int count = 2;
 		bool approx=true;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("frame_id", frameId_, frameId_);
 		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
 		pnh.param("approx_sync", approx, approx);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("count", count, count);
 		pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
 		pnh.param("xyz_output", xyzOutput_, xyzOutput_);
@@ -121,6 +123,8 @@ private:
 			if(approx)
 			{
 				approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
+				if(approxSyncMaxInterval > 0.0)
+					approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 				approxSync4_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, _1, _2, _3, _4));
 			}
 			else
@@ -128,9 +132,10 @@ private:
 				exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
 				exactSync4_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, _1, _2, _3, _4));
 			}
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s,\n   %s",
 					getName().c_str(),
 					approx?"approx":"exact",
+					approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 					cloudSub_1_.getTopic().c_str(),
 					cloudSub_2_.getTopic().c_str(),
 					cloudSub_3_.getTopic().c_str(),
@@ -142,6 +147,8 @@ private:
 			if(approx)
 			{
 				approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
+				if(approxSyncMaxInterval > 0.0)
+					approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 				approxSync3_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, _1, _2, _3));
 			}
 			else
@@ -149,9 +156,10 @@ private:
 				exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
 				exactSync3_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, _1, _2, _3));
 			}
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
 					getName().c_str(),
 					approx?"approx":"exact",
+					approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 					cloudSub_1_.getTopic().c_str(),
 					cloudSub_2_.getTopic().c_str(),
 					cloudSub_3_.getTopic().c_str());
@@ -161,6 +169,8 @@ private:
 			if(approx)
 			{
 				approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
+				if(approxSyncMaxInterval > 0.0)
+					approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 				approxSync2_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, _1, _2));
 			}
 			else
@@ -168,9 +178,10 @@ private:
 				exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
 				exactSync2_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, _1, _2));
 			}
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s",
 					getName().c_str(),
 					approx?"approx":"exact",
+					approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 					cloudSub_1_.getTopic().c_str(),
 					cloudSub_2_.getTopic().c_str());
 		}

src/nodelets/point_cloud_xyz.cpp

@@ -103,7 +103,9 @@ private:
 		int queueSize = 10;
 		bool approxSync = true;
 		std::string roiStr;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("max_depth", maxDepth_, maxDepth_);
 		pnh.param("min_depth", minDepth_, minDepth_);
@@ -170,9 +172,13 @@ private:
 		if(approxSync)
 		{
 			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
 
 			approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
 		}
 		else

src/nodelets/point_cloud_xyzrgb.cpp

@@ -111,7 +111,9 @@ private:
 		int queueSize = 10;
 		bool approxSync = true;
 		std::string roiStr;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("max_depth", maxDepth_, maxDepth_);
 		pnh.param("min_depth", minDepth_, minDepth_);
@@ -195,14 +197,19 @@ private:
 
 		if(approxSync)
 		{
-
 			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
 
 			approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3));
 
 			approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncStereo_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
 		}
 		else

src/nodelets/rgb_sync.cpp

@@ -88,11 +88,15 @@ private:
 
 		int queueSize = 10;
 		bool approxSync = false;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("compressed_rate", compressedRate_, compressedRate_);
 
 		NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
 		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
 		NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
 
@@ -102,6 +106,8 @@ private:
 		if(approxSync)
 		{
 			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, _1, _2));
 		}
 		else
@@ -118,9 +124,10 @@ private:
 		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), 1, hintsRgb);
 		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
 
-		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s",
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s",
 							getName().c_str(),
 							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 							imageSub_.getTopic().c_str(),
 							cameraInfoSub_.getTopic().c_str());
 

src/nodelets/rgbd_odometry.cpp

@@ -123,7 +123,9 @@ private:
 		int rgbdCameras = 1;
 		bool approxSync = true;
 		bool subscribeRGBD = false;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
 		if(pnh.hasParam("depth_cameras"))
@@ -142,6 +144,8 @@ private:
 		pnh.param("keep_color", keepColor_, keepColor_);
 
 		NODELET_INFO("RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
 		NODELET_INFO("RGBDOdometry: queue_size     = %d", queueSize_);
 		NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
 		NODELET_INFO("RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
@@ -175,6 +179,8 @@ private:
 								MyApproxSync2Policy(queueSize_),
 								rgbd_image1_sub_,
 								rgbd_image2_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 						approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
 					}
 					else
@@ -185,9 +191,10 @@ private:
 								rgbd_image2_sub_);
 						exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
 					}
-					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s",
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s",
 							getName().c_str(),
 							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 							rgbd_image1_sub_.getTopic().c_str(),
 							rgbd_image2_sub_.getTopic().c_str());
 				}
@@ -200,6 +207,8 @@ private:
 								rgbd_image1_sub_,
 								rgbd_image2_sub_,
 								rgbd_image3_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 						approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
 					}
 					else
@@ -211,9 +220,10 @@ private:
 								rgbd_image3_sub_);
 						exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
 					}
-					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s",
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s",
 							getName().c_str(),
 							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 							rgbd_image1_sub_.getTopic().c_str(),
 							rgbd_image2_sub_.getTopic().c_str(),
 							rgbd_image3_sub_.getTopic().c_str());
@@ -228,6 +238,8 @@ private:
 								rgbd_image2_sub_,
 								rgbd_image3_sub_,
 								rgbd_image4_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 						approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
 					}
 					else
@@ -240,9 +252,10 @@ private:
 								rgbd_image4_sub_);
 						exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
 					}
-					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
 							getName().c_str(),
 							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 							rgbd_image1_sub_.getTopic().c_str(),
 							rgbd_image2_sub_.getTopic().c_str(),
 							rgbd_image3_sub_.getTopic().c_str(),
@@ -258,7 +271,9 @@ private:
 								rgbd_image2_sub_,
 								rgbd_image3_sub_,
 								rgbd_image4_sub_,
-                                                                rgbd_image5_sub_);
+                                rgbd_image5_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 						approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
 					}
 					else
@@ -269,12 +284,13 @@ private:
 								rgbd_image2_sub_,
 								rgbd_image3_sub_,
 								rgbd_image4_sub_,
-                                                                rgbd_image5_sub_);
+								rgbd_image5_sub_);
 						exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
 					}
-					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n  %s \\\n  %s \\\n   %s \\\n   %s",
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n  %s \\\n  %s \\\n   %s \\\n   %s",
 							getName().c_str(),
 							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 							rgbd_image1_sub_.getTopic().c_str(),
 							rgbd_image2_sub_.getTopic().c_str(),
 							rgbd_image3_sub_.getTopic().c_str(),
@@ -311,6 +327,8 @@ private:
 			if(approxSync)
 			{
 				approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+				if(approxSyncMaxInterval > 0.0)
+					approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 				approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 			}
 			else
@@ -319,9 +337,10 @@ private:
 				exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 			}
 
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s",
 					getName().c_str(),
 					approxSync?"approx":"exact",
+					approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),
 					info_sub_.getTopic().c_str());
@@ -379,6 +398,7 @@ private:
 		cv::Mat depth;
 		pcl::PointCloud<pcl::PointXYZ> scanCloud;
 		std::vector<CameraModel> cameraModels;
+		double stampDiff = 0;
 		for(unsigned int i=0; i<rgbImages.size(); ++i)
 		{
 			if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -392,13 +412,13 @@ private:
 				!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
 				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
-				 {
+			{
 	 				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
 	 				"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
 	 					rgbImages[i]->encoding.c_str(),
 	 					depthImages[i]->encoding.c_str());
 	 				return;
-	 			}
+	 		}
 			UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
 					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
 							imageWidth,
@@ -429,6 +449,29 @@ private:
 				return;
 			}
 
+			if(i>0)
+			{
+				double stampDiff = fabs(rgbImages[i]->header.stamp.toSec() - rgbImages[i-1]->header.stamp.toSec());
+				if(stampDiff > 1.0/60.0)
+				{
+					static bool warningShown = false;
+					if(!warningShown)
+					{
+						NODELET_WARN("The time difference between cameras %d and %d is "
+								"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
+								"to set approx_sync_max_interval to reject bad synchronizations or use "
+								"approx_sync=false if streams have all the exact same timestamp. This "
+								"message is only printed once.",
+								i-1, i,
+								stampDiff,
+								i-1, rgbImages[i-1]->header.stamp.toSec(),
+								i, rgbImages[i]->header.stamp.toSec());
+						warningShown = true;
+					}
+				}
+			}
+
+
 			cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
 			if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
 			   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
@@ -507,6 +550,18 @@ private:
 			depthMsgs[0] = cv_bridge::toCvShare(depth);
 			infoMsgs.push_back(*cameraInfo);
 
+			double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
+			if(stampDiff > 0.010)
+			{
+				NODELET_WARN("The time difference between rgb and depth frames is "
+						"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
+						"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
+						"approx_sync=false if streams have all the exact same timestamp.",
+						stampDiff,
+						image->header.stamp.toSec(),
+						depth->header.stamp.toSec());
+			}
+
 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
 	}

src/nodelets/rgbd_sync.cpp

@@ -92,7 +92,9 @@ private:
 
 		int queueSize = 10;
 		bool approxSync = true;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("depth_scale", depthScale_, depthScale_);
 		pnh.param("decimation", decimation_, decimation_);
@@ -104,6 +106,8 @@ private:
 		}
 
 		NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
 		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
 		NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
 		NODELET_INFO("%s: decimation = %d", getName().c_str(), decimation_);
@@ -115,6 +119,8 @@ private:
 		if(approxSync)
 		{
 			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
 		}
 		else
@@ -136,9 +142,10 @@ private:
 		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
 		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
 
-		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s",
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s",
 							getName().c_str(),
 							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 							imageSub_.getTopic().c_str(),
 							imageDepthSub_.getTopic().c_str(),
 							cameraInfoSub_.getTopic().c_str());
@@ -178,6 +185,18 @@ private:
 			double depthStamp = depth->header.stamp.toSec();
 			double infoStamp = cameraInfo->header.stamp.toSec();
 
+			double stampDiff = fabs(rgbStamp - depthStamp);
+			if(stampDiff > 0.010)
+			{
+				NODELET_WARN("The time difference between rgb and depth frames is "
+						"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
+						"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
+						"approx_sync=false if streams have all the exact same timestamp.",
+						stampDiff,
+						rgbStamp,
+						depthStamp);
+			}
+
 			rtabmap_ros::RGBDImage msg;
 			msg.header.frame_id = cameraInfo->header.frame_id;
 			msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;

src/nodelets/rgbdicp_odometry.cpp

@@ -110,7 +110,9 @@ private:
 
 		bool approxSync = true;
 		bool subscribeScanCloud = false;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
@@ -126,6 +128,8 @@ private:
 		pnh.param("keep_color", keepColor_, keepColor_);
 
 		NODELET_INFO("RGBDIcpOdometry: approx_sync           = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
 		NODELET_INFO("RGBDIcpOdometry: queue_size            = %d", queueSize_);
 		NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud  = %s", subscribeScanCloud?"true":"false");
 		NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
@@ -154,6 +158,8 @@ private:
 			if(approxSync)
 			{
 				approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+				if(approxSyncMaxInterval > 0.0)
+					approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 				approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
 			}
 			else
@@ -162,9 +168,10 @@ private:
 				exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
 			}
 
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s, \n   %s",
 					getName().c_str(),
 					approxSync?"approx":"exact",
+					approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),
 					info_sub_.getTopic().c_str(),
@@ -176,6 +183,8 @@ private:
 			if(approxSync)
 			{
 				approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+				if(approxSyncMaxInterval > 0.0)
+					approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 				approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
 			}
 			else
@@ -184,9 +193,10 @@ private:
 				exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
 			}
 
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
 					getName().c_str(),
 					approxSync?"approx":"exact",
+					approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),
 					info_sub_.getTopic().c_str(),
@@ -277,6 +287,18 @@ private:
 				return;
 			}
 
+			double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
+			if(stampDiff > 0.010)
+			{
+				NODELET_WARN("The time difference between rgb and depth frames is "
+						"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
+						"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
+						"approx_sync=false if streams have all the exact same timestamp.",
+						stampDiff,
+						image->header.stamp.toSec(),
+						depth->header.stamp.toSec());
+			}
+
 			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
 			{
 				rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);

src/nodelets/rgbdx_sync.cpp

@@ -78,11 +78,15 @@ private:
 		int queueSize = 10;
 		bool approxSync = true;
 		int rgbdCameras = 2;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
 
 		NODELET_INFO("%s: approx_sync  = %s", getName().c_str(), approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
 		NODELET_INFO("%s: queue_size   = %d", getName().c_str(), queueSize);
 		NODELET_INFO("%s: rgbd_cameras = %d", getName().c_str(), rgbdCameras);
 
@@ -102,34 +106,63 @@ private:
 		if(rgbdCameras==2)
 		{
 			SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd2ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
 		}
 		else if(rgbdCameras==3)
 		{
 			SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd3ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
 		}
 		else if(rgbdCameras==4)
 		{
 			SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd4ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
 		}
 		else if(rgbdCameras==5)
 		{
 			SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd5ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
 		}
 		else if(rgbdCameras==6)
 		{
 			SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd6ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
 		}
 		else if(rgbdCameras==7)
 		{
 			SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd7ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
 		}
 		else if(rgbdCameras==8)
 		{
 			SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd8ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
 		}
 
 		warningThread_ = new boost::thread(boost::bind(&RGBDXSync::warningLoop, this, subscribedTopicsMsg_, approxSync));
-		NODELET_INFO("%s", subscribedTopicsMsg_.c_str());
+		NODELET_INFO("%s%s", subscribedTopicsMsg_.c_str(),
+				approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
 	}
 
 	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)

src/nodelets/stereo_odometry.cpp

@@ -88,12 +88,16 @@ private:
 
 		bool approxSync = false;
 		bool subscribeRGBD = false;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
 		pnh.param("keep_color", keepColor_, keepColor_);
 
 		NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
 		NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
 		NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
 		NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
@@ -127,6 +131,8 @@ private:
 			if(approxSync)
 			{
 				approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				if(approxSyncMaxInterval>0.0)
+					approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 				approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
 			}
 			else
@@ -136,9 +142,10 @@ private:
 			}
 
 
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
 					getName().c_str(),
 					approxSync?"approx":"exact",
+					approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 					imageRectLeft_.getTopic().c_str(),
 					imageRectRight_.getTopic().c_str(),
 					cameraInfoLeft_.getTopic().c_str(),
@@ -196,6 +203,18 @@ private:
 				return;
 			}
 
+			double stampDiff = fabs(imageRectLeft->header.stamp.toSec() - imageRectRight->header.stamp.toSec());
+			if(stampDiff > 0.010)
+			{
+				NODELET_WARN("The time difference between left and right frames is "
+						"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
+						"synchronized, use approx_sync:=false. Otherwise, you may want "
+						"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
+						stampDiff,
+						imageRectLeft->header.stamp.toSec(),
+						imageRectRight->header.stamp.toSec());
+			}
+
 			int quality = -1;
 			if(imageRectLeft->data.size() && imageRectRight->data.size())
 			{

src/nodelets/stereo_sync.cpp

@@ -88,11 +88,15 @@ private:
 
 		int queueSize = 10;
 		bool approxSync = false;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		if(approxSync)
+			pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("compressed_rate", compressedRate_, compressedRate_);
 
 		NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
+		NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
 		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
 		NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
 
@@ -102,6 +106,8 @@ private:
 		if(approxSync)
 		{
 			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
+			if(approxSyncMaxInterval>0.0)
+				approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, _1, _2, _3, _4));
 		}
 		else
@@ -124,9 +130,10 @@ private:
 		cameraInfoLeftSub_.subscribe(left_nh, "camera_info", 1);
 		cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
 
-		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
 							getName().c_str(),
 							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 							imageLeftSub_.getTopic().c_str(),
 							imageRightSub_.getTopic().c_str(),
 							cameraInfoLeftSub_.getTopic().c_str(),
@@ -169,6 +176,18 @@ private:
 			double leftInfoStamp = cameraInfoLeft->header.stamp.toSec();
 			double rightInfoStamp = cameraInfoRight->header.stamp.toSec();
 
+			double stampDiff = fabs(leftStamp - rightStamp);
+			if(stampDiff > 0.010)
+			{
+				NODELET_WARN("The time difference between left and right frames is "
+						"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
+						"synchronized, use approx_sync:=false. Otherwise, you may want "
+						"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
+						stampDiff,
+						leftStamp,
+						rightStamp);
+			}
+
 			rtabmap_ros::RGBDImage msg;
 			msg.header.frame_id = cameraInfoLeft->header.frame_id;
 			msg.header.stamp = imageLeft->header.stamp>imageRight->header.stamp?imageLeft->header.stamp:imageRight->header.stamp;

src/nodelets/stereo_throttle.cpp

@@ -88,18 +88,24 @@ private:
 
 		int queueSize = 5;
 		bool approxSync = false;
+		double approxSyncMaxInterval = 0.0;
 		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("rate", rate_, rate_);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("decimation", decimation_, decimation_);
 		ROS_ASSERT(decimation_ >= 1);
-		NODELET_INFO("Rate=%f Hz", rate_);
+		NODELET_INFO("rate=%f Hz", rate_);
-		NODELET_INFO("Decimation=%d", decimation_);
+		NODELET_INFO("decimation=%d", decimation_);
-		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
+		NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
 
 		if(approxSync)
 		{
 			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			if(approxSyncMaxInterval>0.0)
+				approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
 			approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
 		}
 		else