Added sync diagnostic (#1026)

* Added sync diagnotic

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

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_;