ROS2 various QOL updates (preparing new binary release) (#1225)

* Updating examples and README * updated main readme * odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning. * update readme * Added husky demo * converted demo_robot_mapping.launch to ros2 * Converted stereo_outdoor demo from ros1 to ros2 * Converted multi-session demo to ros2 * Converted demo_find_object launch to ros2 * renamed files * uniformized default qos, increased sync_queue_size default to 10 * Added vlp16 + imu example * Added husky 3D lidar demos * moved demos in subdir * Added vlp16+zed example * rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo. * forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo * pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces * Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them. * Added isaac sim demo * Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty. * Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo. * rtabmap_demos: added README with examples * Added TOC * bump version to 0.21.9
2026-10-09 11:17:03 +08:00 · 2024-11-30 17:28:16 -08:00
parent e9aa8ed082
commit 3f6adad463
116 changed files with 6102 additions and 575 deletions
@@ -32,7 +32,7 @@ namespace rtabmap_sync {

 CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
 		topicQueueSize_(10),
-		syncQueueSize_(2),
+		syncQueueSize_(10),
 		approxSync_(true),
 		subscribedToDepth_(!gui),
 		subscribedToStereo_(false),
@@ -393,7 +393,7 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
 	}
 	syncQueueSize_ = node.declare_parameter("sync_queue_size", syncQueueSize_);

-	int qos = node.declare_parameter("qos", 0);
+	int qos = node.declare_parameter("qos", (int)RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT);
 	int qosOdom = node.declare_parameter("qos_odom", qos);
 	int qosImage = node.declare_parameter("qos_image", qos);
 	int qosCameraInfo = node.declare_parameter("qos_camera_info", qosImage);
@@ -1104,7 +1104,7 @@ void CommonDataSubscriber::tick(const rclcpp::Time & stamp, double targetFrequen
 {
 	if(syncDiagnostic_.get())
 	{
-		syncDiagnostic_->tick(stamp, targetFrequency);
+		syncDiagnostic_->tickOutput(stamp, targetFrequency);
 	}
 }

@@ -35,6 +35,7 @@ void CommonDataSubscriber::depthCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // Null
@@ -48,6 +49,7 @@ void CommonDataSubscriber::depthScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
@@ -60,6 +62,7 @@ void CommonDataSubscriber::depthScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -72,6 +75,7 @@ void CommonDataSubscriber::depthScanDescCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -88,6 +92,7 @@ void CommonDataSubscriber::depthInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -101,6 +106,7 @@ void CommonDataSubscriber::depthScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
@@ -113,6 +119,7 @@ void CommonDataSubscriber::depthScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -125,6 +132,7 @@ void CommonDataSubscriber::depthScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
@@ -142,6 +150,7 @@ void CommonDataSubscriber::depthOdomCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
@@ -155,6 +164,7 @@ void CommonDataSubscriber::depthOdomScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -167,6 +177,7 @@ void CommonDataSubscriber::depthOdomScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -179,6 +190,7 @@ void CommonDataSubscriber::depthOdomScanDescCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
@@ -195,6 +207,7 @@ void CommonDataSubscriber::depthOdomInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
@@ -208,6 +221,7 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -220,6 +234,7 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -232,6 +247,7 @@ void CommonDataSubscriber::depthOdomScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
 	if(!scanMsg->global_descriptor.data.empty())
@@ -249,6 +265,7 @@ void CommonDataSubscriber::depthDataCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
@@ -262,6 +279,7 @@ void CommonDataSubscriber::depthDataScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -274,6 +292,7 @@ void CommonDataSubscriber::depthDataScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -286,6 +305,7 @@ void CommonDataSubscriber::depthDataScanDescCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
@@ -302,6 +322,7 @@ void CommonDataSubscriber::depthDataInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
@@ -315,6 +336,7 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -327,6 +349,7 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -339,6 +362,7 @@ void CommonDataSubscriber::depthDataScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
 	if(!scanMsg->global_descriptor.data.empty())
@@ -356,6 +380,7 @@ void CommonDataSubscriber::depthOdomDataCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scanMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -369,6 +394,7 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -381,6 +407,7 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -393,6 +420,7 @@ void CommonDataSubscriber::depthOdomDataScanDescCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
 	if(!scanMsg->global_descriptor.data.empty())
@@ -409,6 +437,7 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
@@ -422,6 +451,7 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -434,6 +464,7 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
 }
@@ -446,6 +477,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
 	if(!scanMsg->global_descriptor.data.empty())
 	{
@@ -32,6 +32,7 @@ namespace rtabmap_sync {
 void CommonDataSubscriber::odomCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
@@ -41,6 +42,7 @@ void CommonDataSubscriber::odomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
@@ -50,6 +52,7 @@ void CommonDataSubscriber::odomDataCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
 }
@@ -58,6 +61,7 @@ void CommonDataSubscriber::odomDataInfoCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
 	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
 }
@@ -34,6 +34,7 @@ void CommonDataSubscriber::rgbCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // Null
@@ -47,6 +48,7 @@ void CommonDataSubscriber::rgbScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -59,6 +61,7 @@ void CommonDataSubscriber::rgbScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -71,6 +74,7 @@ void CommonDataSubscriber::rgbScanDescCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
@@ -87,6 +91,7 @@ void CommonDataSubscriber::rgbInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -100,6 +105,7 @@ void CommonDataSubscriber::rgbScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -112,6 +118,7 @@ void CommonDataSubscriber::rgbScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -124,6 +131,7 @@ void CommonDataSubscriber::rgbScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -141,6 +149,7 @@ void CommonDataSubscriber::rgbOdomCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -154,6 +163,7 @@ void CommonDataSubscriber::rgbOdomScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
@@ -166,6 +176,7 @@ void CommonDataSubscriber::rgbOdomScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
@@ -178,6 +189,7 @@ void CommonDataSubscriber::rgbOdomScanDescCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -194,6 +206,7 @@ void CommonDataSubscriber::rgbOdomInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -207,6 +220,7 @@ void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -219,6 +233,7 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -231,6 +246,7 @@ void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
@@ -248,6 +264,7 @@ void CommonDataSubscriber::rgbDataCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -261,6 +278,7 @@ void CommonDataSubscriber::rgbDataScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
@@ -273,6 +291,7 @@ void CommonDataSubscriber::rgbDataScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
@@ -285,6 +304,7 @@ void CommonDataSubscriber::rgbDataScanDescCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -301,6 +321,7 @@ void CommonDataSubscriber::rgbDataInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -314,6 +335,7 @@ void CommonDataSubscriber::rgbDataScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -326,6 +348,7 @@ void CommonDataSubscriber::rgbDataScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -338,6 +361,7 @@ void CommonDataSubscriber::rgbDataScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
@@ -355,6 +379,7 @@ void CommonDataSubscriber::rgbOdomDataCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scanMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
@@ -368,6 +393,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -380,6 +406,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -392,6 +419,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
@@ -408,6 +436,7 @@ void CommonDataSubscriber::rgbOdomDataInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
@@ -421,6 +450,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -433,6 +463,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -445,6 +476,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
 	if(!scanMsg->global_descriptor.data.empty())
@@ -36,6 +36,7 @@ namespace rtabmap_sync {
 void CommonDataSubscriber::rgbdCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -61,6 +62,7 @@ void CommonDataSubscriber::rgbdScan2dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -85,6 +87,7 @@ void CommonDataSubscriber::rgbdScan3dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -109,6 +112,7 @@ void CommonDataSubscriber::rgbdScanDescCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -132,6 +136,7 @@ void CommonDataSubscriber::rgbdInfoCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -158,6 +163,7 @@ void CommonDataSubscriber::rgbdOdomCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -183,6 +189,7 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -207,6 +214,7 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -231,6 +239,7 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -258,6 +267,7 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -284,6 +294,7 @@ void CommonDataSubscriber::rgbdDataCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -309,6 +320,7 @@ void CommonDataSubscriber::rgbdDataScan2dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -333,6 +345,7 @@ void CommonDataSubscriber::rgbdDataScan3dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -357,6 +370,7 @@ void CommonDataSubscriber::rgbdDataScanDescCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -384,6 +398,7 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -410,6 +425,7 @@ void CommonDataSubscriber::rgbdOdomDataCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -435,6 +451,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -459,6 +476,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -483,6 +501,7 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -510,6 +529,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync {

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

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

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

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

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

 #define IMAGE_CONVERSION() \
+		if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imagesMsg->header.stamp);} \
 		UASSERT(!imagesMsg->rgbd_images.empty()); \
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(imagesMsg->rgbd_images.size()); \
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(imagesMsg->rgbd_images.size()); \
@@ -32,6 +32,7 @@ namespace rtabmap_sync {
 void CommonDataSubscriber::scan2dCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
@@ -41,6 +42,7 @@ void CommonDataSubscriber::scan2dCallback(
 void CommonDataSubscriber::scan3dCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -50,6 +52,7 @@ void CommonDataSubscriber::scan3dCallback(
 void CommonDataSubscriber::scanDescCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -59,6 +62,7 @@ void CommonDataSubscriber::scan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
@@ -68,6 +72,7 @@ void CommonDataSubscriber::scan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -77,6 +82,7 @@ void CommonDataSubscriber::scanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
@@ -86,6 +92,7 @@ void CommonDataSubscriber::odomScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -95,6 +102,7 @@ void CommonDataSubscriber::odomScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -104,6 +112,7 @@ void CommonDataSubscriber::odomScanDescCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
@@ -113,6 +122,7 @@ void CommonDataSubscriber::odomScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -122,6 +132,7 @@ void CommonDataSubscriber::odomScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -131,6 +142,7 @@ void CommonDataSubscriber::odomScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
@@ -140,6 +152,7 @@ void CommonDataSubscriber::dataScan2dCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -149,6 +162,7 @@ void CommonDataSubscriber::dataScan3dCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
@@ -158,6 +172,7 @@ void CommonDataSubscriber::dataScanDescCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
@@ -167,6 +182,7 @@ void CommonDataSubscriber::dataScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -176,6 +192,7 @@ void CommonDataSubscriber::dataScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -185,6 +202,7 @@ void CommonDataSubscriber::dataScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
@@ -194,6 +212,7 @@ void CommonDataSubscriber::odomDataScan2dCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
@@ -203,6 +222,7 @@ void CommonDataSubscriber::odomDataScan3dCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
@@ -212,6 +232,7 @@ void CommonDataSubscriber::odomDataScanDescCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
@@ -221,6 +242,7 @@ void CommonDataSubscriber::odomDataScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -230,6 +252,7 @@ void CommonDataSubscriber::odomDataScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
 }
@@ -239,6 +262,7 @@ void CommonDataSubscriber::odomDataScanDescInfoCallback(
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
 	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 #endif
@@ -34,33 +34,37 @@ namespace rtabmap_sync {

 // SensorData
 void CommonDataSubscriber::sensorDataCallback(
-		const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg)
+		const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+	commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::sensorDataInfoCallback(
-		const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg,
+		const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);}
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+	commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg);
 }
 // SensorData + Odom
 void CommonDataSubscriber::sensorDataOdomCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg)
+		const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);}
 	rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+	commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::sensorDataOdomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_msgs::msg::SensorData::ConstSharedPtr imagesMsg,
+		const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);}
+	commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg);
 }

 void CommonDataSubscriber::setupSensorDataCallbacks(
@@ -36,6 +36,7 @@ void CommonDataSubscriber::stereoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // null
@@ -50,6 +51,7 @@ void CommonDataSubscriber::stereoInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);}
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -65,6 +67,7 @@ void CommonDataSubscriber::stereoOdomCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -79,6 +82,7 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);}
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -52,9 +52,9 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
 	exactSync_(0)
 {
 	int topicQueueSize = 10;
-	int syncQueueSize = 2;
+	int syncQueueSize = 10;
 	bool approxSync = true;
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	double approxSyncMaxInterval = 0.0;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
 	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
@@ -138,7 +138,7 @@ void RGBSync::callback(
 		  const sensor_msgs::msg::Image::ConstSharedPtr image,
 		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
-	syncDiagnostic_->tick(image->header.stamp);
+	syncDiagnostic_->tickInput(image->header.stamp);
 	if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
 	{
 		double stamp = rtabmap_conversions::timestampFromROS(image->header.stamp);
@@ -188,6 +188,7 @@ void RGBSync::callback(
 					stamp, rtabmap_conversions::timestampFromROS(image->header.stamp));
 		}
 	}
+	syncDiagnostic_->tickOutput(image->header.stamp);
 }

 }
@@ -50,16 +50,16 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	depthScale_(1.0),
 	decimation_(1),
 	compressedRate_(0),
+	approxSyncMaxInterval_(0.0),
 	approxSyncDepth_(0),
 	exactSyncDepth_(0)
 {
 	int topicQueueSize = 10;
-	int syncQueueSize = 2;
+	int syncQueueSize = 10;
 	bool approxSync = true;
-	double approxSyncMaxInterval = 0.0;
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
-	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
+	approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_);
 	topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
 	int queueSize = this->declare_parameter("queue_size", -1);
 	if(queueSize != -1)
@@ -84,7 +84,7 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :

 	RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
 	if(approxSync)
-		RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
+		RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval_);
 	RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size  = %d", get_name(), topicQueueSize);
 	RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size  = %d", get_name(), syncQueueSize);
 	RCLCPP_INFO(this->get_logger(), "%s: qos             = %d", get_name(), qos);
@@ -99,8 +99,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	if(approxSync)
 	{
 		approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
-		if(approxSyncMaxInterval > 0.0)
-			approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+		if(approxSyncMaxInterval_ > 0.0)
+			approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
 		approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	}
 	else
@@ -109,15 +109,18 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 		exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	}

-	image_transport::TransportHints hints(this);
-	imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
-	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+  	std::string rgbImageTransport = this->declare_parameter<std::string>("rgb_image_transport", "raw");
+	std::string depthImageTransport = this->declare_parameter<std::string>("depth_image_transport", "raw");
+	std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
+	std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
+	imageSub_.subscribe(this, rgbTopic, rgbImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	imageDepthSub_.subscribe(this, depthTopic, depthImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
 	cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());

 	std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
 						get_name(),
 						approxSync?"approx":"exact",
-						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
 						imageSub_.getSubscriber().getTopic().c_str(),
 						imageDepthSub_.getSubscriber().getTopic().c_str(),
 						cameraInfoSub_.getSubscriber()->get_topic_name());
@@ -149,19 +152,20 @@ void RGBDSync::callback(
 		  const sensor_msgs::msg::Image::ConstSharedPtr depth,
 		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
-	syncDiagnostic_->tick(image->header.stamp);
+	syncDiagnostic_->tickInput(image->header.stamp);
 	if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
 	{
 		double rgbStamp = rtabmap_conversions::timestampFromROS(image->header.stamp);
 		double depthStamp = rtabmap_conversions::timestampFromROS(depth->header.stamp);

 		double stampDiff = fabs(rgbStamp - depthStamp);
-		if(stampDiff > 0.010)
+		if(stampDiff > 0.010 && approxSyncMaxInterval_ == 0.0)
 		{
 			RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is "
 					"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
 					"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
-					"approx_sync=false if streams have all the exact same timestamp.",
+					"approx_sync=false if streams have all the exact same timestamp. Setting approx_sync_max_interval "
+					"will suppress this warning.",
 					stampDiff,
 					rgbStamp,
 					depthStamp);
@@ -273,6 +277,7 @@ void RGBDSync::callback(
 					depthStamp, rtabmap_conversions::timestampFromROS(depth->header.stamp));
 		}
 	}
+	syncDiagnostic_->tickOutput(image->header.stamp);
 }

 }
@@ -43,11 +43,11 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
 	SYNC_INIT(rgbd8)
 {
 	int topicQueueSize = 10;
-	int syncQueueSize = 2;
+	int syncQueueSize = 10;
 	bool approxSync = true;
 	int rgbdCameras = 2;
 	double approxSyncMaxInterval = 0.0;
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
 	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
 	topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
@@ -176,13 +176,14 @@ void RGBDXSync::rgbd2Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1)
 {
-	syncDiagnostic_->tick(image0->header.stamp);
+	syncDiagnostic_->tickInput(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(2);
 	output.rgbd_images[0]=(*image0);
 	output.rgbd_images[1]=(*image1);
 	rgbdImagesPub_->publish(output);
+	syncDiagnostic_->tickOutput(image0->header.stamp);
 }

 void RGBDXSync::rgbd3Callback(
@@ -190,7 +191,7 @@ void RGBDXSync::rgbd3Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
 {
-	syncDiagnostic_->tick(image0->header.stamp);
+	syncDiagnostic_->tickInput(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(3);
@@ -198,6 +199,7 @@ void RGBDXSync::rgbd3Callback(
 	output.rgbd_images[1]=(*image1);
 	output.rgbd_images[2]=(*image2);
 	rgbdImagesPub_->publish(output);
+	syncDiagnostic_->tickOutput(image0->header.stamp);
 }

 void RGBDXSync::rgbd4Callback(
@@ -206,7 +208,7 @@ void RGBDXSync::rgbd4Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
 {
-	syncDiagnostic_->tick(image0->header.stamp);
+	syncDiagnostic_->tickInput(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(4);
@@ -215,6 +217,7 @@ void RGBDXSync::rgbd4Callback(
 	output.rgbd_images[2]=(*image2);
 	output.rgbd_images[3]=(*image3);
 	rgbdImagesPub_->publish(output);
+	syncDiagnostic_->tickOutput(image0->header.stamp);
 }

 void RGBDXSync::rgbd5Callback(
@@ -224,7 +227,7 @@ void RGBDXSync::rgbd5Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
 {
-	syncDiagnostic_->tick(image0->header.stamp);
+	syncDiagnostic_->tickInput(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(5);
@@ -234,6 +237,7 @@ void RGBDXSync::rgbd5Callback(
 	output.rgbd_images[3]=(*image3);
 	output.rgbd_images[4]=(*image4);
 	rgbdImagesPub_->publish(output);
+	syncDiagnostic_->tickOutput(image0->header.stamp);
 }

 void RGBDXSync::rgbd6Callback(
@@ -244,7 +248,7 @@ void RGBDXSync::rgbd6Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
 {
-	syncDiagnostic_->tick(image0->header.stamp);
+	syncDiagnostic_->tickInput(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(6);
@@ -255,6 +259,7 @@ void RGBDXSync::rgbd6Callback(
 	output.rgbd_images[4]=(*image4);
 	output.rgbd_images[5]=(*image5);
 	rgbdImagesPub_->publish(output);
+	syncDiagnostic_->tickOutput(image0->header.stamp);
 }

 void RGBDXSync::rgbd7Callback(
@@ -266,7 +271,7 @@ void RGBDXSync::rgbd7Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
 {
-	syncDiagnostic_->tick(image0->header.stamp);
+	syncDiagnostic_->tickInput(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(7);
@@ -278,6 +283,7 @@ void RGBDXSync::rgbd7Callback(
 	output.rgbd_images[5]=(*image5);
 	output.rgbd_images[6]=(*image6);
 	rgbdImagesPub_->publish(output);
+	syncDiagnostic_->tickOutput(image0->header.stamp);
 }

 void RGBDXSync::rgbd8Callback(
@@ -290,7 +296,7 @@ void RGBDXSync::rgbd8Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7)
 {
-	syncDiagnostic_->tick(image0->header.stamp);
+	syncDiagnostic_->tickInput(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(8);
@@ -303,6 +309,7 @@ void RGBDXSync::rgbd8Callback(
 	output.rgbd_images[6]=(*image6);
 	output.rgbd_images[7]=(*image7);
 	rgbdImagesPub_->publish(output);
+	syncDiagnostic_->tickOutput(image0->header.stamp);
 }

 }
@@ -51,10 +51,10 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 		exactSync_(0)
 {
 	int topicQueueSize = 10;
-	int syncQueueSize = 2;
+	int syncQueueSize = 10;
 	bool approxSync = false;
 	double approxSyncMaxInterval = 0.0;
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
 	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
 	topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
@@ -139,7 +139,7 @@ void StereoSync::callback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
 {
-	syncDiagnostic_->tick(imageLeft->header.stamp);
+	syncDiagnostic_->tickInput(imageLeft->header.stamp);
 	if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
 	{
 		double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp);
@@ -209,6 +209,7 @@ void StereoSync::callback(
 					rightStamp, rtabmap_conversions::timestampFromROS(imageRight->header.stamp));
 		}
 	}
+	syncDiagnostic_->tickOutput(imageLeft->header.stamp);
 }

 }