Added sync diagnostic (#1026)

* Added sync diagnotic

* Removed composite task to avoid empty msg, added odom status task

rtabmap_odom/CMakeLists.txt

@@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS
              cv_bridge image_geometry laser_geometry message_filters 
              nav_msgs nodelet pcl_conversions pcl_ros pluginlib roscpp 
              sensor_msgs rtabmap_conversions rtabmap_msgs rtabmap_util
+             rtabmap_sync
 )
 
 catkin_package(
@@ -13,6 +14,7 @@ catkin_package(
   CATKIN_DEPENDS cv_bridge image_geometry laser_geometry message_filters 
              nav_msgs nodelet pcl_conversions pcl_ros pluginlib roscpp 
              sensor_msgs rtabmap_conversions rtabmap_msgs rtabmap_util
+             rtabmap_sync
 )
 
 ###########

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_msgs/Header.h>
 #include <sensor_msgs/Imu.h>
 
+#include <diagnostic_updater/diagnostic_updater.h>
+
 #include <rtabmap_msgs/ResetPose.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
@@ -45,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/thread.hpp>
 
 #include "rtabmap_util/ULogToRosout.h"
+#include "rtabmap_sync/SyncDiagnostic.h"
 
 namespace rtabmap {
 class Odometry;
@@ -52,7 +55,7 @@ class Odometry;
 
 namespace rtabmap_odom {
 
-class OdometryROS : public nodelet::Nodelet
+class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
 {
 
 public:
@@ -77,8 +80,7 @@ public:
 	bool isPaused() const {return paused_;}
 
 protected:
-	void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
-	void callbackCalled() {callbackCalled_ = true;}
+	void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
 
 	virtual void flushCallbacks() = 0;
 	tf::TransformListener & tfListener() {return tfListener_;}
@@ -88,7 +90,6 @@ protected:
 	virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {}
 
 private:
-	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
 	virtual void onInit();
 	virtual void onOdomInit() = 0;
 	virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
@@ -98,8 +99,6 @@ private:
 
 private:
 	rtabmap::Odometry * odometry_;
-	boost::thread * warningThread_;
-	bool callbackCalled_;
 
 	// parameters
 	std::string frameId_;
@@ -154,6 +153,17 @@ private:
 	std::pair<rtabmap::SensorData, std_msgs::Header > bufferedData_;
 
 	rtabmap_util::ULogToRosout ulogToRosout_;
+
+	class OdomStatusTask : public diagnostic_updater::DiagnosticTask
+	{
+	public:
+		OdomStatusTask();
+		void setStatus(bool isLost);
+		void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
+	private:
+		bool lost_;
+	};
+	OdomStatusTask statusDiagnostic_;
 };
 
 }

rtabmap_odom/package.xml

@@ -25,6 +25,7 @@
   <depend>rtabmap_conversions</depend>
   <depend>rtabmap_msgs</depend>
   <depend>rtabmap_util</depend>
+  <depend>rtabmap_sync</depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />

rtabmap_odom/src/OdometryROS.cpp

@@ -57,9 +57,8 @@ using namespace rtabmap;
 namespace rtabmap_odom {
 
 OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
+	rtabmap_sync::SyncDiagnostic(0.5),
 	odometry_(0),
-	warningThread_(0),
-	callbackCalled_(false),
 	frameId_("base_link"),
 	odomFrameId_("odom"),
 	groundTruthFrameId_(""),
@@ -91,13 +90,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 
 OdometryROS::~OdometryROS()
 {
-	if(warningThread_)
-	{
-		callbackCalled();
-		warningThread_->join();
-		delete warningThread_;
-	}
-
 	delete odometry_;
 }
 
@@ -378,29 +370,20 @@ void OdometryROS::onInit()
 	onOdomInit();
 }
 
-void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
+void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic)
 {
-	warningThread_ = new boost::thread(boost::bind(&OdometryROS::warningLoop, this, subscribedTopicsMsg, approxSync));
 	NODELET_INFO("%s", subscribedTopicsMsg.c_str());
-}
-
-void OdometryROS::warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
-{
-	ros::Duration r(5.0);
-	while(!callbackCalled_)
-	{
-		r.sleep();
-		if(!callbackCalled_)
-		{
-			ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+	std::vector<diagnostic_updater::DiagnosticTask*> tasks;
+	tasks.push_back(&statusDiagnostic_);
+	initDiagnostic(subscribedTopic,
+		uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
 					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
 					"header are set. %s%s",
 					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());
-		}
-	}
+					subscribedTopicsMsg.c_str()),
+		tasks);
 }
 
 rtabmap::Transform OdometryROS::velocityGuess() const
@@ -953,6 +936,17 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 		{
 			NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
 		}
+
+		statusDiagnostic_.setStatus(pose.isNull());
+		if(!pose.isNull())
+		{
+			double curentRate = 1.0/(ros::WallTime::now()-time).toSec();
+			tick(header.stamp,
+				maxUpdateRate_>0 && maxUpdateRate_ < curentRate ? maxUpdateRate_:
+				expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
+				previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate);
+		}
+
 		previousStamp_ = header.stamp.toSec();
 	}
 }
@@ -1038,5 +1032,26 @@ bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon
 	return true;
 }
 
+OdometryROS::OdomStatusTask::OdomStatusTask() :
+		diagnostic_updater::DiagnosticTask("Odom status"),
+		lost_(false)
+{}
+
+void OdometryROS::OdomStatusTask::setStatus(bool isLost)
+{
+	lost_ = isLost;
+}
+
+void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+	if(lost_)
+	{
+		stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Lost!");
+	}
+	else
+	{
+		stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Tracking.");
+	}
+}
 
 }

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -157,6 +157,11 @@ private:
 		cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
 
 		filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
+
+		initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
+					getName().c_str(),
+					scan_sub_.getTopic().c_str(),
+					cloud_sub_.getTopic().c_str()), true);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -329,6 +334,7 @@ private:
 			scan_sub_.shutdown();
 			return;
 		}
+
 		scanReceived_ = true;
 		if(this->isPaused())
 		{
@@ -570,6 +576,7 @@ private:
 			cloud_sub_.shutdown();
 			return;
 		}
+
 		cloudReceived_ = true;
 		if(this->isPaused())
 		{

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -161,6 +161,7 @@ private:
 		NODELET_INFO("RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
 		NODELET_INFO("RGBDOdometry: keep_color     = %s", keepColor_?"true":"false");
 
+		std::string subscribedTopic;
 		std::string subscribedTopicsMsg;
 		if(subscribeRGBD)
 		{
@@ -356,6 +357,7 @@ private:
 				exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
 			}
 
+			subscribedTopic = rgb_nh.resolveName("image");
 			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s",
 					getName().c_str(),
 					approxSync?"approx":"exact",
@@ -364,7 +366,7 @@ private:
 					image_depth_sub_.getTopic().c_str(),
 					info_sub_.getTopic().c_str());
 		}
-		this->startWarningThread(subscribedTopicsMsg, approxSync);
+		initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -546,7 +548,6 @@ private:
 			const sensor_msgs::ImageConstPtr& depth,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -575,7 +576,6 @@ private:
 	void callbackRGBD(
 			const rtabmap_msgs::RGBDImageConstPtr& image)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -591,7 +591,6 @@ private:
 	void callbackRGBDX(
 			const rtabmap_msgs::RGBDImagesConstPtr& images)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			if(images->rgbd_images.empty())
@@ -616,7 +615,6 @@ private:
 			const rtabmap_msgs::RGBDImageConstPtr& image,
 			const rtabmap_msgs::RGBDImageConstPtr& image2)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
@@ -636,7 +634,6 @@ private:
 			const rtabmap_msgs::RGBDImageConstPtr& image2,
 			const rtabmap_msgs::RGBDImageConstPtr& image3)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
@@ -659,7 +656,6 @@ private:
 			const rtabmap_msgs::RGBDImageConstPtr& image3,
 			const rtabmap_msgs::RGBDImageConstPtr& image4)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
@@ -685,7 +681,6 @@ private:
 			const rtabmap_msgs::RGBDImageConstPtr& image4,
 			const rtabmap_msgs::RGBDImageConstPtr& image5)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);

rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp

@@ -202,7 +202,7 @@ private:
 					info_sub_.getTopic().c_str(),
 					scan_sub_.getTopic().c_str());
 		}
-		this->startWarningThread(subscribedTopicsMsg, approxSync);
+		initDiagnosticMsg(subscribedTopicsMsg, approxSync);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -243,7 +243,6 @@ 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 ||

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -111,6 +111,7 @@ private:
 		NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
 		NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
 
+		std::string subscribedTopic;
 		std::string subscribedTopicsMsg;
 		if(subscribeRGBD)
 		{
@@ -271,7 +272,7 @@ private:
 				exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
 			}
 
-
+			subscribedTopic = left_nh.resolveName("image_rect");
 			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
 					getName().c_str(),
 					approxSync?"approx":"exact",
@@ -282,7 +283,7 @@ private:
 					cameraInfoRight_.getTopic().c_str());
 		}
 
-		this->startWarningThread(subscribedTopicsMsg, approxSync);
+		initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
 	}
 
 	virtual void updateParameters(ParametersMap & parameters)
@@ -578,7 +579,6 @@ private:
 				const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
 				const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -609,7 +609,6 @@ private:
 	void callbackRGBD(
 			const rtabmap_msgs::RGBDImageConstPtr& image)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -627,7 +626,6 @@ private:
 	void callbackRGBDX(
 			const rtabmap_msgs::RGBDImagesConstPtr& images)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			if(images->rgbd_images.empty())
@@ -654,7 +652,6 @@ private:
 			const rtabmap_msgs::RGBDImageConstPtr& image,
 			const rtabmap_msgs::RGBDImageConstPtr& image2)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
@@ -677,7 +674,6 @@ private:
 			const rtabmap_msgs::RGBDImageConstPtr& image2,
 			const rtabmap_msgs::RGBDImageConstPtr& image3)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
@@ -704,7 +700,6 @@ private:
 			const rtabmap_msgs::RGBDImageConstPtr& image3,
 			const rtabmap_msgs::RGBDImageConstPtr& image4)
 	{
-		callbackCalled();
 		if(!this->isPaused())
 		{
 			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);

rtabmap_slam/src/CoreWrapper.cpp

@@ -2245,6 +2245,12 @@ void CoreWrapper::process(
 		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeUpdatingMaps/ms"), timeUpdateMaps*1000.0f));
 		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimePublishing/ms"), timePublishMaps*1000.0f));
 		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeTotal/ms"), (timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps)*1000.0f));
+
+		// If not intermediate node
+		if(data.id() > 0)
+		{
+			tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps));
+		}
 	}
 	else if(!rtabmap_.isIDsGenerated())
 	{

rtabmap_sync/CMakeLists.txt

@@ -3,7 +3,7 @@ project(rtabmap_sync)
 
 find_package(catkin REQUIRED COMPONENTS
             cv_bridge roscpp sensor_msgs nav_msgs image_transport
-            nodelet message_filters rtabmap_msgs rtabmap_conversions
+            nodelet message_filters rtabmap_msgs rtabmap_conversions diagnostic_updater
 )
 
 option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support"  OFF)
@@ -33,7 +33,7 @@ catkin_package(
   INCLUDE_DIRS include
   LIBRARIES rtabmap_sync rtabmap_sync_plugins
   CATKIN_DEPENDS cv_bridge roscpp sensor_msgs nav_msgs image_transport
-                 nodelet message_filters rtabmap_msgs rtabmap_conversions
+                 nodelet message_filters rtabmap_msgs rtabmap_conversions diagnostic_updater
   CFG_EXTRAS extra_configs.cmake
 )
 

rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h

@@ -51,12 +51,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_msgs/OdomInfo.h>
 #include <rtabmap_msgs/ScanDescriptor.h>
 #include <rtabmap_sync/CommonDataSubscriberDefines.h>
+#include <rtabmap_sync/SyncDiagnostic.h>
 
 #include <boost/thread.hpp>
 
 namespace rtabmap_sync {
 
-class CommonDataSubscriber {
+class CommonDataSubscriber : public SyncDiagnostic {
 public:
 	CommonDataSubscriber(bool gui);
 	virtual ~CommonDataSubscriber();
@@ -122,8 +123,6 @@ protected:
 				const cv::Mat & localDescriptors = cv::Mat());
 
 private:
-	void warningLoop();
-	void callbackCalled() {callbackCalled_ = true;}
 	void setupDepthCallbacks(
 			ros::NodeHandle & nh,
 			ros::NodeHandle & pnh,
@@ -256,8 +255,6 @@ protected:
 
 private:
 	bool approxSync_;
-	boost::thread* warningThread_;
-	bool callbackCalled_;
 	bool subscribedToDepth_;
 	bool subscribedToStereo_;
 	bool subscribedToRGB_;

rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h

@@ -0,0 +1,81 @@
+#ifndef INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
+#define INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
+
+#include "rtabmap/utilite/UStl.h"
+
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
+
+namespace rtabmap_sync {
+
+class SyncDiagnostic {
+    public:
+        SyncDiagnostic(double tolerance = 0.1) :
+            frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
+            lastCallbackCalledStamp_(ros::Time::now().toSec()-1),
+            targetFrequency_(0.0)
+    {}
+
+protected:
+    void initDiagnostic(
+        const std::string & topic,
+        const std::string & topicsNotReceivedWarningMsg,
+		std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
+    {
+        topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg;
+
+		std::list<std::string> strList = uSplit(topic, '/');
+		for(int i=0; i<2 && strList.size()>1; ++i)
+		{
+			// Assuming format is /back_camera/left/image, we want "back_camera"
+			strList.pop_back();
+		}
+		diagnosticUpdater_.add(frequencyStatus_);
+		for(size_t i=0; i<otherTasks.size(); ++i)
+		{
+			diagnosticUpdater_.add(*otherTasks[i]);
+		}
+		diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
+		diagnosticUpdater_.force_update();
+		diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this);
+    }
+
+    void tick(const ros::Time & stamp, double targetFrequency = 0)
+    {
+        frequencyStatus_.tick();
+		double period = stamp.toSec() - lastCallbackCalledStamp_;
+		if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_))
+		{
+			targetFrequency_ = 1.0/period;
+		}
+        else if(targetFrequency>0)
+        {
+            targetFrequency_ = targetFrequency;
+        }
+		lastCallbackCalledStamp_ = stamp.toSec();
+    }
+
+private:
+    void diagnosticTimerCallback(const ros::TimerEvent& event)
+    {
+        diagnosticUpdater_.update();
+
+        if(ros::Time::now().toSec()-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
+        {
+            ROS_WARN_THROTTLE(5, topicsNotReceivedWarningMsg_.c_str());
+        }	
+    }
+
+    private:
+        std::string topicsNotReceivedWarningMsg_;
+        diagnostic_updater::Updater diagnosticUpdater_;
+        diagnostic_updater::FrequencyStatus frequencyStatus_;
+        ros::Timer diagnosticTimer_;
+        double lastCallbackCalledStamp_;
+        double targetFrequency_;
+
+};
+
+}
+
+#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */

rtabmap_sync/package.xml

@@ -20,6 +20,7 @@
   <depend>rtabmap_conversions</depend>
   <depend>rtabmap_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>diagnostic_updater</depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />

rtabmap_sync/src/CommonDataSubscriber.cpp

@@ -30,10 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync {
 
 CommonDataSubscriber::CommonDataSubscriber(bool gui) :
+		SyncDiagnostic(0.5),
 		queueSize_(10),
 		approxSync_(true),
-		warningThread_(0),
-		callbackCalled_(false),
 		subscribedToDepth_(!gui),
 		subscribedToStereo_(false),
 		subscribedToRGB_(!gui),
@@ -661,20 +660,23 @@ void CommonDataSubscriber::setupCallbacks(
 
 	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
 	{
-		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
 		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
+		initDiagnostic("",
+			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+					"header are set. If topics are coming from different computers, make sure "
+					"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
+					name_.c_str(),
+					approxSync_?
+							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
+							"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()));
+		
 	}
 }
 
 CommonDataSubscriber::~CommonDataSubscriber()
 {
-	if(warningThread_)
-	{
-		callbackCalled();
-		warningThread_->join();
-		delete warningThread_;
-	}
-
 	// RGB + Depth
 	SYNC_DEL(depth);
 	SYNC_DEL(depthScan2d);
@@ -990,27 +992,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	rgbdSubs_.clear();
 }
 
-void CommonDataSubscriber::warningLoop()
-{
-	ros::Duration r(5.0);
-	while(!callbackCalled_)
-	{
-		r.sleep();
-		if(!callbackCalled_)
-		{
-			ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
-					"header are set. If topics are coming from different computers, make sure "
-					"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
-					name_.c_str(),
-					approxSync_?
-							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
-							"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::commonSingleCameraCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
@@ -1026,8 +1007,6 @@ void CommonDataSubscriber::commonSingleCameraCallback(
 		const std::vector<rtabmap_msgs::Point3f> & localPoints3d,
 		const cv::Mat & localDescriptors)
 {
-	callbackCalled();
-
 	std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPointsMsgs;
 	localKeyPointsMsgs.push_back(localKeyPoints);
 	std::vector<std::vector<rtabmap_msgs::Point3f> > localPoints3dMsgs;

rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp

@@ -32,7 +32,6 @@ namespace rtabmap_sync {
 void CommonDataSubscriber::odomCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
@@ -42,7 +41,6 @@ void CommonDataSubscriber::odomInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
@@ -52,7 +50,6 @@ void CommonDataSubscriber::odomDataCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
 }
@@ -61,7 +58,6 @@ void CommonDataSubscriber::odomDataInfoCallback(
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
 }

rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp

@@ -35,7 +35,6 @@ namespace rtabmap_sync {
 
 #define IMAGE_CONVERSION() \
 		UASSERT(!imagesMsg->rgbd_images.empty()); \
-		callbackCalled(); \
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(imagesMsg->rgbd_images.size()); \
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(imagesMsg->rgbd_images.size()); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \

rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp

@@ -32,7 +32,6 @@ namespace rtabmap_sync {
 void CommonDataSubscriber::scan2dCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
@@ -42,7 +41,6 @@ void CommonDataSubscriber::scan2dCallback(
 void CommonDataSubscriber::scan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScan scan2dMsg; // Null
@@ -52,7 +50,6 @@ void CommonDataSubscriber::scan3dCallback(
 void CommonDataSubscriber::scanDescCallback(
 		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
@@ -62,7 +59,6 @@ void CommonDataSubscriber::scan2dInfoCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
@@ -72,7 +68,6 @@ void CommonDataSubscriber::scan3dInfoCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScan scan2dMsg; // Null
@@ -82,7 +77,6 @@ void CommonDataSubscriber::scanDescInfoCallback(
 		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
@@ -92,7 +86,6 @@ void CommonDataSubscriber::odomScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
@@ -102,7 +95,6 @@ void CommonDataSubscriber::odomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
@@ -112,7 +104,6 @@ void CommonDataSubscriber::odomScanDescCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
 		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
@@ -122,7 +113,6 @@ void CommonDataSubscriber::odomScan2dInfoCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -132,7 +122,6 @@ void CommonDataSubscriber::odomScan3dInfoCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScan scan2dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -142,7 +131,6 @@ void CommonDataSubscriber::odomScanDescInfoCallback(
 		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
@@ -152,7 +140,6 @@ void CommonDataSubscriber::dataScan2dCallback(
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
-	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
@@ -162,7 +149,6 @@ void CommonDataSubscriber::dataScan3dCallback(
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
-	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
@@ -172,7 +158,6 @@ void CommonDataSubscriber::dataScanDescCallback(
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
 		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
 {
-	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
@@ -182,7 +167,6 @@ void CommonDataSubscriber::dataScan2dInfoCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -192,7 +176,6 @@ void CommonDataSubscriber::dataScan3dInfoCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::LaserScan scan2dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -202,7 +185,6 @@ void CommonDataSubscriber::dataScanDescInfoCallback(
 		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
@@ -212,7 +194,6 @@ void CommonDataSubscriber::odomDataScan2dCallback(
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
-	callbackCalled();
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -222,7 +203,6 @@ void CommonDataSubscriber::odomDataScan3dCallback(
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
-	callbackCalled();
 	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -232,7 +212,6 @@ void CommonDataSubscriber::odomDataScanDescCallback(
 		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
 		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
@@ -242,7 +221,6 @@ void CommonDataSubscriber::odomDataScan2dInfoCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -252,7 +230,6 @@ void CommonDataSubscriber::odomDataScan3dInfoCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	sensor_msgs::LaserScan scan2dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
 }
@@ -262,7 +239,6 @@ void CommonDataSubscriber::odomDataScanDescInfoCallback(
 		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
 		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 #endif

rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp

@@ -32,11 +32,10 @@ namespace rtabmap_sync {
 // Stereo
 void CommonDataSubscriber::stereoCallback(
 		const sensor_msgs::ImageConstPtr& leftImageMsg,
-	   const sensor_msgs::ImageConstPtr& rightImageMsg,
-	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
 {
-	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScan scanMsg; // null
@@ -46,12 +45,11 @@ void CommonDataSubscriber::stereoCallback(
 }
 void CommonDataSubscriber::stereoInfoCallback(
 		const sensor_msgs::ImageConstPtr& leftImageMsg,
-	   const sensor_msgs::ImageConstPtr& rightImageMsg,
-	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-	   const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
 {
-	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScan scan2dMsg; // null
@@ -67,7 +65,6 @@ void CommonDataSubscriber::stereoOdomCallback(
 		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
 		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScan scanMsg; // Null
 	sensor_msgs::PointCloud2 scan3dMsg; // null
@@ -82,7 +79,6 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
 		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 		const rtabmap_msgs::OdomInfoConstPtr & odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScan scan2dMsg; // Null
 	sensor_msgs::PointCloud2 scan3dMsg; // Null

rtabmap_sync/src/nodelets/rgb_sync.cpp

@@ -51,33 +51,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/UConversion.h"
 
+#include "rtabmap_sync/SyncDiagnostic.h"
+
 namespace rtabmap_sync
 {
 
-class RgbSync : public nodelet::Nodelet
+class RgbSync : public nodelet::Nodelet, public SyncDiagnostic
 {
 public:
 	RgbSync() :
 		compressedRate_(0),
-		warningThread_(0),
-		callbackCalled_(false),
 		approxSync_(0),
 		exactSync_(0)
 	{}
 
 	virtual ~RgbSync()
 	{
-		if(approxSync_)
-			delete approxSync_;
-		if(exactSync_)
-			delete exactSync_;
-
-		if(warningThread_)
-		{
-			callbackCalled_=true;
-			warningThread_->join();
-			delete warningThread_;
-		}
+		delete approxSync_;
+		delete exactSync_;
 	}
 
 private:
@@ -130,35 +121,24 @@ private:
 							approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 							imageSub_.getTopic().c_str(),
 							cameraInfoSub_.getTopic().c_str());
+		NODELET_INFO(subscribedTopicsMsg.c_str());
 
-		warningThread_ = new boost::thread(boost::bind(&RgbSync::warningLoop, this, subscribedTopicsMsg, approxSync));
-		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
-	}
-
-	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
-	{
-		ros::Duration r(5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
-						"header are set. %s%s",
-						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());
-			}
-		}
+		initDiagnostic(rgb_nh.resolveName("image_rect"),
+			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+					"header are set. %s%s",
+					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 callback(
 			  const sensor_msgs::ImageConstPtr& image,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
-		callbackCalled_ = true;
+		tick(image->header.stamp);
+
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
 			double stamp = image->header.stamp.toSec();
@@ -212,8 +192,6 @@ private:
 
 private:
 	double compressedRate_;
-	boost::thread * warningThread_;
-	bool callbackCalled_;
 	ros::Time lastCompressedPublished_;
 
 	ros::Publisher rgbdImagePub_;

rtabmap_sync/src/nodelets/rgbd_sync.cpp

@@ -53,35 +53,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/utilite/UConversion.h"
 
+#include "rtabmap_sync/SyncDiagnostic.h"
+
 namespace rtabmap_sync
 {
 
-class RGBDSync : public nodelet::Nodelet
+class RGBDSync : public nodelet::Nodelet, public SyncDiagnostic
 {
 public:
 	RGBDSync() :
 		depthScale_(1.0),
 		decimation_(1),
 		compressedRate_(0),
-		warningThread_(0),
-		callbackCalled_(false),
 		approxSyncDepth_(0),
 		exactSyncDepth_(0)
 	{}
 
 	virtual ~RGBDSync()
 	{
-		if(approxSyncDepth_)
-			delete approxSyncDepth_;
-		if(exactSyncDepth_)
-			delete exactSyncDepth_;
-
-		if(warningThread_)
-		{
-			callbackCalled_=true;
-			warningThread_->join();
-			delete warningThread_;
-		}
+		delete approxSyncDepth_;
+		delete exactSyncDepth_;
 	}
 
 private:
@@ -149,28 +140,16 @@ private:
 							imageSub_.getTopic().c_str(),
 							imageDepthSub_.getTopic().c_str(),
 							cameraInfoSub_.getTopic().c_str());
+		NODELET_INFO(subscribedTopicsMsg.c_str());
 
-		warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync));
-		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
-	}
-
-	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
-	{
-		ros::Duration r(5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
-						"header are set. %s%s",
-						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());
-			}
-		}
+		initDiagnostic(rgb_nh.resolveName("image"), 
+			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+					"header are set. %s%s",
+					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 callback(
@@ -178,7 +157,8 @@ private:
 			  const sensor_msgs::ImageConstPtr& depth,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
-		callbackCalled_ = true;
+		tick(image->header.stamp);
+
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
 			double rgbStamp = image->header.stamp.toSec();
@@ -309,8 +289,6 @@ private:
 	double depthScale_;
 	int decimation_;
 	double compressedRate_;
-	boost::thread * warningThread_;
-	bool callbackCalled_;
 
 	ros::Time lastCompressedPublished_;
 

rtabmap_sync/src/nodelets/rgbdx_sync.cpp

@@ -41,12 +41,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
-class RGBDXSync : public nodelet::Nodelet
+class RGBDXSync : public nodelet::Nodelet, public SyncDiagnostic
 {
 public:
 	RGBDXSync() :
-		warningThread_(0),
-		callbackCalled_(false),
 		SYNC_INIT(rgbd2),
 		SYNC_INIT(rgbd3),
 		SYNC_INIT(rgbd4),
@@ -59,12 +57,16 @@ public:
 	virtual ~RGBDXSync()
 	{
 		SYNC_DEL(rgbd2);
+		SYNC_DEL(rgbd3);
+		SYNC_DEL(rgbd4);
+		SYNC_DEL(rgbd5);
+		SYNC_DEL(rgbd6);
+		SYNC_DEL(rgbd7);
+		SYNC_DEL(rgbd8);
 
-		if(warningThread_)
+		for(size_t i=0; i<rgbdSubs_.size(); ++i)
 		{
-			callbackCalled_=true;
-			warningThread_->join();
-			delete warningThread_;
+			delete rgbdSubs_[i];
 		}
 	}
 
@@ -160,28 +162,19 @@ private:
 			}
 		}
 
-		warningThread_ = new boost::thread(boost::bind(&RGBDXSync::warningLoop, this, subscribedTopicsMsg_, approxSync));
-		NODELET_INFO("%s%s", subscribedTopicsMsg_.c_str(),
+		std::string subscribedTopicsMsg = uFormat("%s%s", subscribedTopicsMsg_.c_str(),
 				approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
-	}
+		NODELET_INFO(subscribedTopicsMsg.c_str());
 
-	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
-	{
-		ros::Duration r(5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
-						"header are set. %s%s",
-						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());
-			}
-		}
+		// Setup diagnostic
+		initDiagnostic("",
+			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+					"header are set. %s%s",
+					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()));
 	}
 
 	DATA_SYNCS2(rgbd2, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
@@ -193,20 +186,16 @@ private:
 	DATA_SYNCS8(rgbd8, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
 
 private:
-	boost::thread * warningThread_;
-	bool callbackCalled_;
-
 	ros::Publisher rgbdImagesPub_;
 
 	std::vector<message_filters::Subscriber<rtabmap_msgs::RGBDImage>*> rgbdSubs_;
 };
 
-
 void RGBDXSync::rgbd2Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image0,
 		  const rtabmap_msgs::RGBDImageConstPtr& image1)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(2);
@@ -220,7 +209,7 @@ void RGBDXSync::rgbd3Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image1,
 		  const rtabmap_msgs::RGBDImageConstPtr& image2)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(3);
@@ -236,7 +225,7 @@ void RGBDXSync::rgbd4Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image2,
 		  const rtabmap_msgs::RGBDImageConstPtr& image3)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(4);
@@ -254,7 +243,7 @@ void RGBDXSync::rgbd5Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image3,
 		  const rtabmap_msgs::RGBDImageConstPtr& image4)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(5);
@@ -274,7 +263,7 @@ void RGBDXSync::rgbd6Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image4,
 		  const rtabmap_msgs::RGBDImageConstPtr& image5)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(6);
@@ -296,7 +285,7 @@ void RGBDXSync::rgbd7Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image5,
 		  const rtabmap_msgs::RGBDImageConstPtr& image6)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(7);
@@ -320,7 +309,7 @@ void RGBDXSync::rgbd8Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image6,
 		  const rtabmap_msgs::RGBDImageConstPtr& image7)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(8);

rtabmap_sync/src/nodelets/stereo_sync.cpp

@@ -51,33 +51,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/UConversion.h"
 
+#include "rtabmap_sync/SyncDiagnostic.h"
+
 namespace rtabmap_sync
 {
 
-class StereoSync : public nodelet::Nodelet
+class StereoSync : public nodelet::Nodelet, public SyncDiagnostic
 {
 public:
 	StereoSync() :
 		compressedRate_(0),
-		warningThread_(0),
-		callbackCalled_(false),
 		approxSync_(0),
 		exactSync_(0)
 	{}
 
 	virtual ~StereoSync()
 	{
-		if(approxSync_)
-			delete approxSync_;
-		if(exactSync_)
-			delete exactSync_;
-
-		if(warningThread_)
-		{
-			callbackCalled_=true;
-			warningThread_->join();
-			delete warningThread_;
-		}
+		delete approxSync_;
+		delete exactSync_;
 	}
 
 private:
@@ -138,28 +129,17 @@ private:
 							imageRightSub_.getTopic().c_str(),
 							cameraInfoLeftSub_.getTopic().c_str(),
 							cameraInfoRightSub_.getTopic().c_str());
+		NODELET_INFO(subscribedTopicsMsg.c_str());
 
-		warningThread_ = new boost::thread(boost::bind(&StereoSync::warningLoop, this, subscribedTopicsMsg, approxSync));
-		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
-	}
+		initDiagnostic(left_nh.resolveName("image_rect"),
+			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+					"header are set. %s%s",
+					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 warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
-	{
-		ros::Duration r(5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
-						"header are set. %s%s",
-						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 callback(
@@ -168,7 +148,8 @@ private:
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
 	{
-		callbackCalled_ = true;
+		tick(imageLeft->header.stamp);
+
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
 			double leftStamp = imageLeft->header.stamp.toSec();
@@ -244,8 +225,6 @@ private:
 
 private:
 	double compressedRate_;
-	boost::thread * warningThread_;
-	bool callbackCalled_;
 	ros::Time lastCompressedPublished_;
 
 	ros::Publisher rgbdImagePub_;

rtabmap_viz/src/GuiWrapper.cpp

@@ -221,6 +221,9 @@ void GuiWrapper::infoMapCallback(
 	stat.setConstraints(links);
 
 	this->post(new RtabmapEvent(stat));
+
+	tick(infoMsg->header.stamp);
+
 }
 
 void GuiWrapper::infoCallback(
@@ -251,6 +254,8 @@ void GuiWrapper::infoCallback(
 	}
 
 	this->post(new RtabmapEvent(stat));
+
+	tick(infoMsg->header.stamp);
 }
 
 void GuiWrapper::goalPathCallback(