Added warning after 10 seconds if any callback has not been called since the start (rtabmap, rtabmapviz and odometry nodes).

include/rtabmap_ros/CommonDataSubscriber.h

@@ -50,6 +50,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap_ros/CommonDataSubscriberDefines.h>
 
+#include <boost/thread.hpp>
+
 namespace rtabmap_ros {
 
 class CommonDataSubscriber {
@@ -97,6 +99,8 @@ protected:
 				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
 
 private:
+	void warningLoop();
+	void callbackCalled() {callbackCalled_ = true;}
 	void setupDepthCallbacks(
 			bool subscribeOdom,
 			bool subscribeUserData,
@@ -127,8 +131,14 @@ private:
 			int queueSize,
 			bool approxSync);
 
+protected:
+	std::string subscribedTopicsMsg_;
+
 private:
 	int queueSize_;
+	bool approxSync_;
+	boost::thread* warningThread_;
+	bool callbackCalled_;
 	bool subscribedToDepth_;
 	bool subscribedToStereo_;
 	bool subscribedToRGBD_;

include/rtabmap_ros/CommonDataSubscriberDefines.h

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
 #define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
 
+#include <rtabmap/utilite/UConversion.h>
 
 #define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
 	typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \
@@ -98,7 +99,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
 		} \
-		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s", \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s", \
 				ros::this_node::getName().c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \
@@ -117,7 +118,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
 		} \
-		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s", \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s", \
 				ros::this_node::getName().c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \
@@ -137,7 +138,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
 		} \
-		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s", \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s", \
 				ros::this_node::getName().c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \
@@ -158,7 +159,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
 		} \
-		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
+		subscribedTopicsMsg_ = uFormat("\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", \
 				SUB0.getTopic().c_str(), \
@@ -180,7 +181,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
 		} \
-		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
 				ros::this_node::getName().c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \

include/rtabmap_ros/OdometryROS.h

@@ -41,6 +41,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 
+#include <boost/thread.hpp>
+
 namespace rtabmap {
 class Odometry;
 }
@@ -72,16 +74,22 @@ public:
 	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
 
 protected:
+	void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
+	void callbackCalled() {callbackCalled_ = true;}
+
 	virtual void flushCallbacks() = 0;
 	tf::TransformListener & tfListener() {return tfListener_;}
 
 private:
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
 	virtual void onInit();
 	virtual void onOdomInit() = 0;
 	virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
 
 private:
 	rtabmap::Odometry * odometry_;
+	boost::thread * warningThread_;
+	bool callbackCalled_;
 
 	// parameters
 	std::string frameId_;

launch/rtabmap.launch

@@ -36,7 +36,10 @@
   <arg name="wait_for_transform"      default="0.2"/>
   <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
   <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
-  <arg name="approx_sync"             default="true"/>         <!-- if timestamps of the input topics are not synchronized -->
+  
+  <!-- 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="true"/>         
    
   <!-- RGB-D related topics -->
   <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
@@ -163,6 +166,7 @@
       <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
       <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
       <param name="queue_size"           type="int" value="$(arg queue_size)"/>
+      <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
     
       <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
       <remap from="depth/image"     to="$(arg depth_topic_relay)"/>

src/CommonDataSubscriber.cpp

@@ -32,6 +32,9 @@ namespace rtabmap_ros {
 
 CommonDataSubscriber::CommonDataSubscriber() :
 		queueSize_(10),
+		approxSync_(true),
+		warningThread_(0),
+		callbackCalled_(false),
 		subscribedToDepth_(true),
 		subscribedToStereo_(false),
 		subscribedToRGBD_(false),
@@ -126,7 +129,6 @@ CommonDataSubscriber::CommonDataSubscriber() :
 	bool subscribeOdomInfo = false;
 	bool subscribeUserData = false;
 	int rgbdCameras = 1;
-	bool approxSync = true;
 
 	// ROS related parameters (private)
 	pnh.param("subscribe_depth",     subscribedToDepth_, subscribedToDepth_);
@@ -170,7 +172,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
 	}
 	if(subscribedToStereo_)
 	{
-		approxSync = false; // default for stereo: exact sync
+		approxSync_ = false; // default for stereo: exact sync
 	}
 
 	std::string odomFrameId;
@@ -186,11 +188,11 @@ CommonDataSubscriber::CommonDataSubscriber() :
 		ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
 				 "to \"approx_sync\"! Your value is still copied to "
 				 "corresponding parameter.");
-		pnh.param("stereo_approx_sync", approxSync, approxSync);
+		pnh.param("stereo_approx_sync", approxSync_, approxSync_);
 	}
 	else
 	{
-		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync", approxSync_, approxSync_);
 	}
 
 	if(rgbdCameras <= 0 && subscribedToRGBD_)
@@ -200,7 +202,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
 
 	ROS_INFO("%s: queue_size    = %d", ros::this_node::getName().c_str(), queueSize_);
 	ROS_INFO("%s: rgbd_cameras = %d", ros::this_node::getName().c_str(), rgbdCameras);
-	ROS_INFO("%s: approx_sync   = %s", ros::this_node::getName().c_str(), approxSync?"true":"false");
+	ROS_INFO("%s: approx_sync   = %s", ros::this_node::getName().c_str(), approxSync_?"true":"false");
 
 	bool subscribeOdom = odomFrameId.empty();
 	if(subscribedToDepth_)
@@ -212,7 +214,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
 				subscribeScan3d,
 				subscribeOdomInfo,
 				queueSize_,
-				approxSync);
+				approxSync_);
 	}
 	else if(subscribedToStereo_)
 	{
@@ -220,7 +222,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
 				subscribeOdom,
 				subscribeOdomInfo,
 				queueSize_,
-				approxSync);
+				approxSync_);
 	}
 	else if(subscribedToRGBD_)
 	{
@@ -233,7 +235,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
 					subscribeScan3d,
 					subscribeOdomInfo,
 					queueSize_,
-					approxSync);
+					approxSync_);
 		}
 		else
 		{
@@ -244,13 +246,23 @@ CommonDataSubscriber::CommonDataSubscriber() :
 					subscribeScan3d,
 					subscribeOdomInfo,
 					queueSize_,
-					approxSync);
+					approxSync_);
 		}
 	}
+	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_)
+	{
+		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
+		ROS_INFO(subscribedTopicsMsg_.c_str());
+	}
 }
 
 CommonDataSubscriber::~CommonDataSubscriber()
 {
+	if(warningThread_)
+	{
+		delete warningThread_;
+	}
+
 	// RGB + Depth
 	SYNC_DEL(depth);
 	SYNC_DEL(depthScan2d);
@@ -337,6 +349,25 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	rgbdSubs_.clear();
 }
 
+void CommonDataSubscriber::warningLoop()
+{
+	ros::Duration r(10.0);
+	while(ros::ok() && !callbackCalled_)
+	{
+		r.sleep();
+		if(ros::ok() && !callbackCalled_)
+		{
+			ROS_WARN("%s: Did not receive data since 10 seconds! Make sure the input topics are "
+					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+					"header are set. %s%s",
+					ros::this_node::getName().c_str(),
+					approxSync_?"":"Parameter \"approx_sync\" is false, which means that input "
+						"topics should have all the exact timestamp for the callback to be called.",
+					subscribedTopicsMsg_.c_str());
+		}
+	}
+}
+
 void CommonDataSubscriber::commonSingleDepthCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -347,6 +378,7 @@ void CommonDataSubscriber::commonSingleDepthCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
+	callbackCalled();
 	std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
 	std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
 	std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;

src/OdometryROS.cpp

@@ -58,6 +58,8 @@ namespace rtabmap_ros {
 
 OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	odometry_(0),
+	warningThread_(0),
+	callbackCalled_(false),
 	frameId_("base_link"),
 	odomFrameId_("odom"),
 	groundTruthFrameId_(""),
@@ -79,6 +81,10 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 
 OdometryROS::~OdometryROS()
 {
+	if(warningThread_)
+	{
+		delete warningThread_;
+	}
 	ros::NodeHandle & pnh = getPrivateNodeHandle();
 	if(pnh.ok())
 	{
@@ -297,6 +303,31 @@ void OdometryROS::onInit()
 	onOdomInit();
 }
 
+void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
+{
+	warningThread_ = new boost::thread(boost::bind(&OdometryROS::warningLoop, this, subscribedTopicsMsg, approxSync));
+	NODELET_INFO(subscribedTopicsMsg.c_str());
+}
+
+void OdometryROS::warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
+{
+	ros::Duration r(10.0);
+	while(ros::ok() && !callbackCalled_)
+	{
+		r.sleep();
+		if(ros::ok() && !callbackCalled_)
+		{
+			ROS_WARN("%s: Did not receive data since 10 seconds! Make sure the input topics are "
+					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+					"header are set. %s%s",
+					ros::this_node::getName().c_str(),
+					approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
+						"topics should have all the exact timestamp for the callback to be called.",
+					subscribedTopicsMsg.c_str());
+		}
+	}
+}
+
 Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
 {
 	// TF ready?

src/impl/CommonDataSubscriberRGBD.cpp

@@ -377,7 +377,8 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 	{
 		rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);
 
-		ROS_INFO("\n%s subscribed to:\n   %s",
+		subscribedTopicsMsg_ =
+				uFormat("\n%s subscribed to:\n   %s",
 				ros::this_node::getName().c_str(),
 				rgbdSub_.getTopic().c_str());
 	}

src/impl/CommonDataSubscriberRGBD2.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_ros {
 
 #define IMAGE_CONVERSION() \
+		callbackCalled(); \
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
 		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \

src/impl/CommonDataSubscriberStereo.cpp

@@ -36,6 +36,7 @@ void CommonDataSubscriber::stereoCallback(
 	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
 {
+	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
@@ -49,6 +50,7 @@ void CommonDataSubscriber::stereoInfoCallback(
 	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 	   const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
+	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
@@ -63,6 +65,7 @@ void CommonDataSubscriber::stereoOdomCallback(
 		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
 {
+	callbackCalled();
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
@@ -76,6 +79,7 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
 		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
 {
+	callbackCalled();
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);

src/nodelets/rgbd_odometry.cpp

@@ -114,6 +114,7 @@ private:
 			NODELET_FATAL("Only 2 cameras maximum supported yet.");
 		}
 
+		std::string subscribedTopicsMsg;
 		if(rgbdCameras == 2)
 		{
 			rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
@@ -135,7 +136,7 @@ private:
 						rgbd_image2_sub_);
 				exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
 			}
-			NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s",
 					ros::this_node::getName().c_str(),
 					approxSync?"approx":"exact",
 					rgbd_image1_sub_.getTopic().c_str(),
@@ -167,13 +168,14 @@ private:
 				exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 			}
 
-			NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
 					ros::this_node::getName().c_str(),
 					approxSync?"approx":"exact",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),
 					info_sub_.getTopic().c_str());
 		}
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -303,6 +305,7 @@ private:
 			const sensor_msgs::ImageConstPtr& depth,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
+		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -320,6 +323,7 @@ private:
 			const rtabmap_ros::RGBDImageConstPtr& image,
 			const rtabmap_ros::RGBDImageConstPtr& image2)
 	{
+		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);

src/nodelets/rgbdicp_odometry.cpp

@@ -125,6 +125,7 @@ private:
 		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
 		info_sub_.subscribe(rgb_nh, "camera_info", 1);
 
+		std::string subscribedTopicsMsg;
 		if(subscribeScanCloud)
 		{
 			cloud_sub_.subscribe(nh, "scan_cloud", 1);
@@ -139,7 +140,7 @@ private:
 				exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
 			}
 
-			NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
 					ros::this_node::getName().c_str(),
 					approxSync?"approx":"exact",
 					image_mono_sub_.getTopic().c_str(),
@@ -161,7 +162,7 @@ private:
 				exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
 			}
 
-			NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
 					ros::this_node::getName().c_str(),
 					approxSync?"approx":"exact",
 					image_mono_sub_.getTopic().c_str(),
@@ -169,6 +170,7 @@ private:
 					info_sub_.getTopic().c_str(),
 					scan_sub_.getTopic().c_str());
 		}
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -219,6 +221,7 @@ private:
 			const sensor_msgs::LaserScanConstPtr& scanMsg,
 			const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
 	{
+		callbackCalled();
 		if(!this->isPaused())
 		{
 			if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||

src/nodelets/stereo_odometry.cpp

@@ -113,13 +113,14 @@ private:
 		}
 
 
-		NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 				ros::this_node::getName().c_str(),
 				approxSync?"approx":"exact",
 				imageRectLeft_.getTopic().c_str(),
 				imageRectRight_.getTopic().c_str(),
 				cameraInfoLeft_.getTopic().c_str(),
 				cameraInfoRight_.getTopic().c_str());
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -139,6 +140,7 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
 	{
+		callbackCalled();
 		if(!this->isPaused())
 		{
 			if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||