merged master->ros2

rtabmap_sync/src/CommonDataSubscriber.cpp

@@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync {
 
 CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
-		SyncDiagnostic(&node, 0.5),
 		queueSize_(10),
 		approxSync_(true),
 		subscribedToDepth_(!gui),
@@ -39,6 +38,7 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
 		subscribedToRGB_(!gui),
 		subscribedToOdom_(true),
 		subscribedToRGBD_(false),
+		subscribedToSensorData_(false),
 		subscribedToScan2d_(false),
 		subscribedToScan3d_(false),
 		subscribedToScanDescriptor_(false),
@@ -195,6 +195,11 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
 		SYNC_INIT(rgbdXOdomDataInfo),
 #endif
 
+		// SensorData
+		SYNC_INIT(sensorDataInfo),
+		SYNC_INIT(sensorDataOdom),
+		SYNC_INIT(sensorDataOdomInfo),
+
 #ifdef RTABMAP_SYNC_MULTI_RGBD
 		// 2 RGBD
 		SYNC_INIT(rgbd2),
@@ -366,6 +371,7 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
 	subscribedToScanDescriptor_ = node.declare_parameter("subscribe_scan_descriptor", subscribedToScanDescriptor_);
 	subscribedToStereo_ = node.declare_parameter("subscribe_stereo", subscribedToStereo_);
 	subscribedToRGBD_ = node.declare_parameter("subscribe_rgbd", subscribedToRGBD_);
+	subscribedToSensorData_ = node.declare_parameter("subscribe_sensor_data", subscribedToSensorData_);
 	subscribedToOdomInfo_ = node.declare_parameter("subscribe_odom_info", subscribedToOdomInfo_);
 	subscribedToUserData_ = node.declare_parameter("subscribe_user_data", subscribedToUserData_);
 	subscribedToOdom_ = node.declare_parameter("subscribe_odom", subscribedToOdom_);
@@ -380,14 +386,18 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
 	int qosCameraInfo = node.declare_parameter("qos_camera_info", qosImage);
 	int qosScan = node.declare_parameter("qos_scan", qos);
 	int qosUserData = node.declare_parameter("qos_user_data", qos);
+	int qosSensorData = node.declare_parameter("qos_sensor_data", qos);
 	qosOdom_ = (rmw_qos_reliability_policy_t)qosOdom;
 	qosImage_ = (rmw_qos_reliability_policy_t)qosImage;
 	qosCameraInfo_ = (rmw_qos_reliability_policy_t)qosCameraInfo;
 	qosScan_ = (rmw_qos_reliability_policy_t)qosScan;
 	qosUserData_ = (rmw_qos_reliability_policy_t)qosUserData;
+	qosSensorData_ = (rmw_qos_reliability_policy_t)qosSensorData;
 }
 
-void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
+void CommonDataSubscriber::setupCallbacks(
+		rclcpp::Node & node,
+		std::vector<diagnostic_updater::DiagnosticTask*> otherTasks)
 {
 #ifndef RTABMAP_SYNC_USER_DATA
 	if(subscribedToUserData_)
@@ -408,21 +418,51 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 		RCLCPP_WARN(node.get_logger(), "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_)
 	{
-		RCLCPP_WARN(node.get_logger(), "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_)
+		{
+			RCLCPP_WARN(node.get_logger(), "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_)
+		{
+			RCLCPP_WARN(node.get_logger(), "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_)
+		{
+			RCLCPP_WARN(node.get_logger(), "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_)
 	{
-		RCLCPP_WARN(node.get_logger(), "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_)
-	{
-		RCLCPP_WARN(node.get_logger(), "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_)
+			{
+				RCLCPP_WARN(node.get_logger(), "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_)
+			{
+				RCLCPP_WARN(node.get_logger(), "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_)
+			{
+				RCLCPP_WARN(node.get_logger(), "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
+		{
+			RCLCPP_WARN(node.get_logger(), "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(subscribedToScan2d_ && subscribedToScan3d_)
 	{
@@ -439,6 +479,21 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 		RCLCPP_WARN(node.get_logger(), "rtabmap: Parameters subscribe_scan_cloud and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
 		subscribedToScan3d_ = false;
 	}
+	if(subscribedToSensorData_ && subscribedToScan2d_)
+	{
+		RCLCPP_WARN(node.get_logger(), "rtabmap: Parameters subscribe_sensor_data and subscribe_scan cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
+		subscribedToScan2d_ = false;
+	}
+	if(subscribedToSensorData_ && subscribedToScan3d_)
+	{
+		RCLCPP_WARN(node.get_logger(), "rtabmap: Parameters subscribe_sensor_data and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
+		subscribedToScan3d_ = false;
+	}
+	if(subscribedToSensorData_ && subscribedToScanDescriptor_)
+	{
+		RCLCPP_WARN(node.get_logger(), "rtabmap: Parameters subscribe_sensor_data and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_descriptor is set to false.");
+		subscribedToScanDescriptor_ = false;
+	}
 	if(subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_)
 	{
 		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
@@ -458,6 +513,7 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 	RCLCPP_INFO(node.get_logger(), "%s: subscribe_rgb = %s", name_.c_str(), subscribedToRGB_?"true":"false");
 	RCLCPP_INFO(node.get_logger(), "%s: subscribe_stereo = %s", name_.c_str(), subscribedToStereo_?"true":"false");
 	RCLCPP_INFO(node.get_logger(), "%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name_.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras_);
+	RCLCPP_INFO(node.get_logger(), "%s: subscribe_sensor_data = %s", name_.c_str(), subscribedToSensorData_?"true":"false");
 	RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom_info = %s", name_.c_str(), subscribedToOdomInfo_?"true":"false");
 	RCLCPP_INFO(node.get_logger(), "%s: subscribe_user_data = %s", name_.c_str(), subscribedToUserData_?"true":"false");
 	RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan = %s", name_.c_str(), subscribedToScan2d_?"true":"false");
@@ -630,6 +686,15 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 					queueSize_,
 					approxSync_);
 	}
+	else if(subscribedToSensorData_)
+	{
+		setupSensorDataCallbacks(
+					node,
+					subscribedToOdom_,
+					subscribedToOdomInfo_,
+					queueSize_,
+					approxSync_);
+	}
 	else if(subscribedToOdom_)
 	{
 		setupOdomCallbacks(
@@ -643,7 +708,8 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
 	{
 		RCLCPP_INFO(node.get_logger(), "%s", subscribedTopicsMsg_.c_str());
-		initDiagnostic("",
+		syncDiagnostic_.reset(new SyncDiagnostic(&node, 0.5));
+		syncDiagnostic_->init("",
 			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
 					"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
 					"header are set. If topics are coming from different computers, make sure "
@@ -652,7 +718,8 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 					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()));
+					subscribedTopicsMsg_.c_str()),
+					otherTasks);
 	}
 }
 
@@ -1025,4 +1092,12 @@ void CommonDataSubscriber::commonSingleCameraCallback(
 			localDescriptorsMsgs);
 }
 
+void CommonDataSubscriber::tick(const rclcpp::Time & stamp, double targetFrequency)
+{
+	if(syncDiagnostic_.get())
+	{
+		syncDiagnostic_->tick(stamp, targetFrequency);
+	}
+}
+
 } /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp

@@ -526,7 +526,8 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
 		}
 		else
 		{
-			rgbdXSubOnly_ = node.create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability(qosOdom_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
+			rgbdXSub_.unsubscribe();
+			rgbdXSubOnly_ = node.create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
 
 			subscribedTopicsMsg_ =
 					uFormat("\n%s subscribed to:\n   %s",

rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp

@@ -0,0 +1,114 @@
+/*
+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::msg::SensorData::ConstSharedPtr imagesMsg)
+{
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::sensorDataInfoCallback(
+		const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg,
+		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+}
+// SensorData + Odom
+void CommonDataSubscriber::sensorDataOdomCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg)
+{
+	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::sensorDataOdomInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg,
+		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupSensorDataCallbacks(
+		rclcpp::Node& node,
+		bool subscribeOdom,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	RCLCPP_INFO(node.get_logger(), "Setup SensorData callback");
+
+	sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(1).reliability(qosSensorData_).get_rmw_qos_profile());
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
+			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(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
+			SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_);
+		}
+		else
+		{
+			sensorDataSub_.unsubscribe();
+			sensorDataSubOnly_ = node.create_subscription<rtabmap_msgs::msg::SensorData>("sensor_data", rclcpp::QoS(1).reliability(qosSensorData_), std::bind(&CommonDataSubscriber::sensorDataCallback, this, std::placeholders::_1));
+
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					node.get_name(),
+					sensorDataSubOnly_->get_topic_name());
+
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/nodelets/rgb_sync.cpp

@@ -43,7 +43,6 @@ namespace rtabmap_sync
 
 RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
 	Node("rgbd_sync", options),
-	SyncDiagnostic(this),
 	compressedRate_(0),
 	approxSync_(0),
 	exactSync_(0)
@@ -96,7 +95,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
 
 	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 
-	initDiagnostic(imageSub_.getSubscriber().getTopic(),
+		syncDiagnostic_.reset(new SyncDiagnostic(this));
+		syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(),
 			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
 					"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
 					"header are set. %s%s",
@@ -119,7 +119,7 @@ void RGBSync::callback(
 		  const sensor_msgs::msg::Image::ConstSharedPtr image,
 		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
-	tick(image->header.stamp);
+	syncDiagnostic_->tick(image->header.stamp);
 	if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
 	{
 		double stamp = rtabmap_conversions::timestampFromROS(image->header.stamp);

rtabmap_sync/src/nodelets/rgbd_sync.cpp

@@ -43,7 +43,6 @@ namespace rtabmap_sync
 
 RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	Node("rgbd_sync", options),
-	SyncDiagnostic(this),
 	depthScale_(1.0),
 	decimation_(1),
 	compressedRate_(0),
@@ -109,14 +108,15 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 
 	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 
-	initDiagnostic(imageSub_.getSubscriber().getTopic(),
-				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",
-						get_name(),
-						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()));
+	syncDiagnostic_.reset(new SyncDiagnostic(this));
+	syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(),
+			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",
+					get_name(),
+					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()));
 }
 
 RGBDSync::~RGBDSync()
@@ -130,7 +130,7 @@ void RGBDSync::callback(
 		  const sensor_msgs::msg::Image::ConstSharedPtr depth,
 		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
-	tick(image->header.stamp);
+	syncDiagnostic_->tick(image->header.stamp);
 	if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
 	{
 		double rgbStamp = rtabmap_conversions::timestampFromROS(image->header.stamp);

rtabmap_sync/src/nodelets/rgbdx_sync.cpp

@@ -34,7 +34,6 @@ namespace rtabmap_sync
 
 RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
 	Node("rgbd_sync", options),
-	SyncDiagnostic(this),
 	SYNC_INIT(rgbd2),
 	SYNC_INIT(rgbd3),
 	SYNC_INIT(rgbd4),
@@ -131,18 +130,20 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
 		}
 	}
 
-	RCLCPP_INFO(this->get_logger(), "%s%s", subscribedTopicsMsg_.c_str(),
-			approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
+	std::string subscribedTopicsMsg = uFormat("%s%s", subscribedTopicsMsg_.c_str(),
+				approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
+	RCLCPP_INFO(this->get_logger(), subscribedTopicsMsg.c_str());
 
 	// Setup diagnostic
-	initDiagnostic("",
+	syncDiagnostic_.reset(new SyncDiagnostic(this));
+	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",
 				get_name(),
 				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()));
 }
 
 RGBDXSync::~RGBDXSync()
@@ -160,7 +161,7 @@ void RGBDXSync::rgbd2Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(2);
@@ -174,7 +175,7 @@ void RGBDXSync::rgbd3Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(3);
@@ -190,7 +191,7 @@ void RGBDXSync::rgbd4Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(4);
@@ -208,7 +209,7 @@ void RGBDXSync::rgbd5Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(5);
@@ -228,7 +229,7 @@ void RGBDXSync::rgbd6Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(6);
@@ -250,7 +251,7 @@ void RGBDXSync::rgbd7Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(7);
@@ -274,7 +275,7 @@ void RGBDXSync::rgbd8Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7)
 {
-	tick(image0->header.stamp);
+	syncDiagnostic_->tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(8);

rtabmap_sync/src/nodelets/stereo_sync.cpp

@@ -42,7 +42,6 @@ namespace rtabmap_sync
 
 StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 		Node("stereo_sync", options),
-		SyncDiagnostic(this),
 		compressedRate_(0),
 		approxSync_(0),
 		exactSync_(0)
@@ -98,7 +97,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 
 	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 
-	initDiagnostic(imageLeftSub_.getSubscriber().getTopic(),
+	syncDiagnostic_.reset(new SyncDiagnostic(this));
+	syncDiagnostic_->init(imageLeftSub_.getSubscriber().getTopic(),
 		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",
@@ -120,7 +120,7 @@ void StereoSync::callback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
 {
-	tick(imageLeft->header.stamp);
+	syncDiagnostic_->tick(imageLeft->header.stamp);
 	if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
 	{
 		double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp);