Add rtabmap_msgs/SensorData (#1055)

* Add rtabmap_msgs/SensorData

* After testing fixes

* Updated output topic name

* added explicit --logconsole for nodelets

* fixed intermediate nodes not generated

* Refactored SyncDiagnostic usage to handle nodelet name

* SyncDiagnostic: added TimeStampStatus. Odom: added new status when data not received yet

* rtabmap: Fixed parameters not updated when using nodelet

rtabmap_sync/src/CommonDataSubscriber.cpp

@@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync {
 
 CommonDataSubscriber::CommonDataSubscriber(bool gui) :
-		SyncDiagnostic(0.5),
 		queueSize_(10),
 		approxSync_(true),
 		subscribedToDepth_(!gui),
@@ -38,6 +37,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		subscribedToRGB_(!gui),
 		subscribedToOdom_(false),
 		subscribedToRGBD_(false),
+		subscribedToSensorData_(false),
 		subscribedToScan2d_(false),
 		subscribedToScan3d_(false),
 		subscribedToScanDescriptor_(false),
@@ -380,6 +380,7 @@ void CommonDataSubscriber::setupCallbacks(
 	pnh.param("subscribe_scan_descriptor", subscribeScanDesc, subscribeScanDesc);
 	pnh.param("subscribe_stereo",    subscribedToStereo_, subscribedToStereo_);
 	pnh.param("subscribe_rgbd",      subscribedToRGBD_, subscribedToRGBD_);
+	pnh.param("subscribe_sensor_data", subscribedToSensorData_, subscribedToSensorData_);
 	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
 	pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
 	pnh.param("subscribe_odom",      subscribeOdom, subscribeOdom);
@@ -403,21 +404,51 @@ void CommonDataSubscriber::setupCallbacks(
 		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgb cannot be true at the same time. Parameter subscribe_rgb is set to false.");
 		subscribedToRGB_ = false;
 	}
-	if(subscribedToDepth_ && subscribedToRGBD_)
+	if(subscribedToRGBD_)
 	{
-		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
-		subscribedToDepth_ = false;
-		subscribedToRGB_ = false;
+		if(subscribedToDepth_)
+		{
+			ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
+			subscribedToDepth_ = false;
+			subscribedToRGB_ = false;
+		}
+		if(subscribedToRGB_)
+		{
+			ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
+			subscribedToRGB_ = false;
+		}
+		if(subscribedToStereo_)
+		{
+			ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
+			subscribedToStereo_ = false;
+		}
 	}
-	if(subscribedToRGB_ && subscribedToRGBD_)
+	if(subscribedToSensorData_)
 	{
-		ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
-		subscribedToRGB_ = false;
-	}
-	if(subscribedToStereo_ && subscribedToRGBD_)
-	{
-		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
-		subscribedToStereo_ = false;
+		if(!subscribedToRGBD_)
+		{
+			if(subscribedToDepth_)
+			{
+				ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_depth is set to false.");
+				subscribedToDepth_ = false;
+				subscribedToRGB_ = false;
+			}
+			if(subscribedToRGB_)
+			{
+				ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_rgb is set to false.");
+				subscribedToRGB_ = false;
+			}
+			if(subscribedToStereo_)
+			{
+				ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_stereo is set to false.");
+				subscribedToStereo_ = false;
+			}
+		}
+		else
+		{
+			ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgbd is set to false.");
+			subscribedToRGBD_ = false;
+		}
 	}
 	if(subscribeScan2d && subscribeScan3d)
 	{
@@ -434,6 +465,21 @@ void CommonDataSubscriber::setupCallbacks(
 		ROS_WARN("rtabmap: Parameters subscribe_scan_cloud and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
 		subscribeScan3d = false;
 	}
+	if(subscribedToSensorData_ && subscribeScan2d)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
+		subscribeScan2d = false;
+	}
+	if(subscribedToSensorData_ && subscribeScan3d)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
+		subscribeScan3d = false;
+	}
+	if(subscribedToSensorData_ && subscribeScanDesc)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_descriptor is set to false.");
+		subscribeScanDesc = false;
+	}
 	if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
 	{
 		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
@@ -470,6 +516,7 @@ void CommonDataSubscriber::setupCallbacks(
 	ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
 	ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
 	ROS_INFO("%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras);
+	ROS_INFO("%s: subscribe_sensor_data = %s", name.c_str(), subscribedToSensorData_?"true":"false");
 	ROS_INFO("%s: subscribe_odom_info = %s", name.c_str(), subscribeOdomInfo?"true":"false");
 	ROS_INFO("%s: subscribe_user_data = %s", name.c_str(), subscribeUserData?"true":"false");
 	ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false");
@@ -648,6 +695,16 @@ void CommonDataSubscriber::setupCallbacks(
 					queueSize_,
 					approxSync_);
 	}
+	else if(subscribedToSensorData_)
+	{
+		setupSensorDataCallbacks(
+					nh,
+					pnh,
+					subscribedToOdom_,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync_);
+	}
 	else if(subscribedToOdom_)
 	{
 		setupOdomCallbacks(
@@ -662,7 +719,8 @@ void CommonDataSubscriber::setupCallbacks(
 	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
 	{
 		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
-		initDiagnostic("",
+		syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, name, 0.5));
+		syncDiagnostic_->init("",
 			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 "
@@ -1046,4 +1104,12 @@ void CommonDataSubscriber::commonSingleCameraCallback(
 			localDescriptorsMsgs);
 }
 
+void CommonDataSubscriber::tick(const ros::Time & stamp, double targetFrequency)
+{
+	if(syncDiagnostic_.get())
+	{
+		syncDiagnostic_->tick(stamp, targetFrequency);
+	}
+}
+
 } /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp

@@ -523,6 +523,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
 		}
 		else
 		{
+			rgbdXSub_.unsubscribe();
 			rgbdXSubOnly_ = nh.subscribe("rgbd_images", queueSize, &CommonDataSubscriber::rgbdXCallback, this);
 
 			subscribedTopicsMsg_ =

rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp

@@ -0,0 +1,113 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_sync {
+
+// SensorData
+void CommonDataSubscriber::sensorDataCallback(
+		const rtabmap_msgs::SensorDataConstPtr& imagesMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::sensorDataInfoCallback(
+		const rtabmap_msgs::SensorDataConstPtr& imagesMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+}
+// SensorData + Odom
+void CommonDataSubscriber::sensorDataOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::SensorDataConstPtr& imagesMsg)
+{
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::sensorDataOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::SensorDataConstPtr& imagesMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupSensorDataCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup SensorData callback");
+
+	sensorDataSub_.subscribe(nh, "sensor_data", queueSize);
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync, queueSize, odomSub_, sensorDataSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync, queueSize, odomSub_, sensorDataSub_);
+		}
+	}
+	else
+	{
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_);
+		}
+		else
+		{
+			sensorDataSub_.unsubscribe();
+			sensorDataSubOnly_ = nh.subscribe("sensor_data", queueSize, &CommonDataSubscriber::sensorDataCallback, this);
+
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					ros::this_node::getName().c_str(),
+					sensorDataSubOnly_.getTopic().c_str());
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/nodelets/rgb_sync.cpp

@@ -56,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
-class RgbSync : public nodelet::Nodelet, public SyncDiagnostic
+class RgbSync : public nodelet::Nodelet
 {
 public:
 	RgbSync() :
@@ -123,7 +123,8 @@ private:
 							cameraInfoSub_.getTopic().c_str());
 		NODELET_INFO(subscribedTopicsMsg.c_str());
 
-		initDiagnostic(rgb_nh.resolveName("image_rect"),
+		syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
+		syncDiagnostic_->init(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",
@@ -137,7 +138,7 @@ private:
 			  const sensor_msgs::ImageConstPtr& image,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
-		tick(image->header.stamp);
+		syncDiagnostic_->tick(image->header.stamp);
 
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
@@ -205,6 +206,8 @@ private:
 
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+
+	std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RgbSync, nodelet::Nodelet);

rtabmap_sync/src/nodelets/rgbd_sync.cpp

@@ -58,7 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
-class RGBDSync : public nodelet::Nodelet, public SyncDiagnostic
+class RGBDSync : public nodelet::Nodelet
 {
 public:
 	RGBDSync() :
@@ -142,7 +142,8 @@ private:
 							cameraInfoSub_.getTopic().c_str());
 		NODELET_INFO(subscribedTopicsMsg.c_str());
 
-		initDiagnostic(rgb_nh.resolveName("image"), 
+		syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
+		syncDiagnostic_->init(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",
@@ -157,7 +158,7 @@ private:
 			  const sensor_msgs::ImageConstPtr& depth,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
-		tick(image->header.stamp);
+		syncDiagnostic_->tick(image->header.stamp);
 
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
@@ -304,6 +305,8 @@ private:
 
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
 	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+
+	std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RGBDSync, nodelet::Nodelet);

rtabmap_sync/src/nodelets/rgbdx_sync.cpp

@@ -41,7 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
-class RGBDXSync : public nodelet::Nodelet, public SyncDiagnostic
+class RGBDXSync : public nodelet::Nodelet
 {
 public:
 	RGBDXSync() :
@@ -167,7 +167,8 @@ private:
 		NODELET_INFO(subscribedTopicsMsg.c_str());
 
 		// Setup diagnostic
-		initDiagnostic("",
+		syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
+		syncDiagnostic_->init("",
 			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",
@@ -189,13 +190,16 @@ private:
 	ros::Publisher rgbdImagesPub_;
 
 	std::vector<message_filters::Subscriber<rtabmap_msgs::RGBDImage>*> rgbdSubs_;
+
+	std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
 };
 
 void RGBDXSync::rgbd2Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image0,
 		  const rtabmap_msgs::RGBDImageConstPtr& image1)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
+
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(2);
@@ -209,7 +213,7 @@ void RGBDXSync::rgbd3Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image1,
 		  const rtabmap_msgs::RGBDImageConstPtr& image2)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(3);
@@ -225,7 +229,7 @@ void RGBDXSync::rgbd4Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image2,
 		  const rtabmap_msgs::RGBDImageConstPtr& image3)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(4);
@@ -243,7 +247,7 @@ void RGBDXSync::rgbd5Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image3,
 		  const rtabmap_msgs::RGBDImageConstPtr& image4)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(5);
@@ -263,7 +267,7 @@ void RGBDXSync::rgbd6Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image4,
 		  const rtabmap_msgs::RGBDImageConstPtr& image5)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(6);
@@ -285,7 +289,7 @@ void RGBDXSync::rgbd7Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image5,
 		  const rtabmap_msgs::RGBDImageConstPtr& image6)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(7);
@@ -309,7 +313,7 @@ void RGBDXSync::rgbd8Callback(
 		  const rtabmap_msgs::RGBDImageConstPtr& image6,
 		  const rtabmap_msgs::RGBDImageConstPtr& image7)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(8);

rtabmap_sync/src/nodelets/stereo_sync.cpp

@@ -56,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
-class StereoSync : public nodelet::Nodelet, public SyncDiagnostic
+class StereoSync : public nodelet::Nodelet
 {
 public:
 	StereoSync() :
@@ -131,7 +131,8 @@ private:
 							cameraInfoRightSub_.getTopic().c_str());
 		NODELET_INFO(subscribedTopicsMsg.c_str());
 
-		initDiagnostic(left_nh.resolveName("image_rect"),
+		syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
+		syncDiagnostic_->init(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",
@@ -148,7 +149,7 @@ private:
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
 	{
-		tick(imageLeft->header.stamp);
+		syncDiagnostic_->tick(imageLeft->header.stamp);
 
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
@@ -240,6 +241,8 @@ private:
 
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+
+	std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_sync::StereoSync, nodelet::Nodelet);