merged master->ros2 (diagnostics, #1046)

.github/workflows/docker.yml

@@ -11,7 +11,7 @@ jobs:
     
     strategy:
       matrix:
-        docker_tag: [kinetic, melodic, melodic-latest, noetic, noetic-latest]
+        docker_tag: [kinetic, melodic, noetic, noetic-latest]
         include:
         - docker_tag: kinetic
           docker_path: 'kinetic'
@@ -23,11 +23,6 @@ jobs:
             linux/amd64
             linux/arm64
 #            linux/arm/v7
-        - docker_tag: melodic-latest
-          docker_path: 'melodic/latest'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
         - docker_tag: noetic
           docker_path: 'noetic'
           docker_platforms: |

docker/noetic/superpoint/Dockerfile

@@ -50,7 +50,7 @@ RUN apt update && \
 # Install ros dependencies
 RUN apt-get update && \
     apt upgrade -y && \
-    apt-get install -y git ros-noetic-ros-base python3-catkin-tools python3-rosdep build-essential ros-noetic-rtabmap-ros && \
+    apt-get install -y git ros-noetic-ros-base python3-catkin-tools python3-rosdep build-essential ros-noetic-rtabmap-ros ros-noetic-pybind11-catkin && \
     apt-get remove -y ros-noetic-rtabmap && \
     rosdep init && \
     apt-get clean && rm -rf /var/lib/apt/lists/

rtabmap_demos/launch/demo_unitree_quadruped_robot.launch

@@ -0,0 +1,95 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <!-- See https://github.com/unitreerobotics/unitree_guide to bringup simulation.
+       Fix Cx/Cy of the cameras by setting them to 464 and 400 respectively in unitree_ros/robots/go1_description/xacro/depthCamera.xacro
+       Ideally, build rtabmap with OpenGV support. 
+       Would work better if simulated environment has a lot of visual texture, see https://github.com/introlab/rtabmap_ros/issues/1031#issuecomment-1722322305 
+       
+       Launch:
+        $ roslaunch unitree_guide gazeboSim.launch wname:=apt
+        $ roslaunch rtabmap_demos demo_unitree_quadruped_robot.launch
+        $ ~/catkin_ws/devel/lib/unitree_guide/junior_ctrl
+       Press 2 to get up, press 4 to move (w,a,s,d) and rotate (j,l)
+       -->
+
+   <arg name="localization" default="false"/>
+   <arg     if="$(arg localization)" name="rtabmap_args" default="--Mem/IncrementalMemory false"/>
+   <arg unless="$(arg localization)" name="rtabmap_args" default="--Mem/IncrementalMemory true --delete_db_on_start"/>
+
+   <!-- sync rgb/depth images and camera info per camera -->
+   <group ns="camera_face">
+    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
+      <remap from="rgb/image"         to="color/image_raw"/>
+      <remap from="depth/image"       to="depth/image_raw"/>
+      <remap from="rgb/camera_info"   to="color/camera_info"/>
+      <param name="approx_sync" value="false"/>
+    </node>
+   </group>
+   <group ns="camera_left">
+    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
+      <remap from="rgb/image"         to="color/image_raw"/>
+      <remap from="depth/image"       to="depth/image_raw"/>
+      <remap from="rgb/camera_info"   to="color/camera_info"/>
+      <param name="approx_sync" value="false"/>
+    </node>
+   </group>
+   <group ns="camera_right">
+    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
+      <remap from="rgb/image"         to="color/image_raw"/>
+      <remap from="depth/image"       to="depth/image_raw"/>
+      <remap from="rgb/camera_info"   to="color/camera_info"/>
+      <param name="approx_sync" value="false"/>
+    </node>
+   </group>
+
+  <group ns="rtabmap">
+
+    <!-- sync all cameras together -->
+    <node pkg="rtabmap_sync" type="rgbdx_sync" name="rgbdx_sync" output="screen">
+      <remap from="rgbd_image0" to="/camera_left/rgbd_image"/>
+      <remap from="rgbd_image1" to="/camera_face/rgbd_image"/>
+      <remap from="rgbd_image2" to="/camera_right/rgbd_image"/>
+      <param name="rgbd_cameras" type="int"   value="3"/>
+    </node>
+
+    <!-- Odometry -->
+    <node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+      <remap from="imu" to="/trunk_imu"/>
+
+	  <param name="subscribe_rgbd"           type="bool"   value="true"/>
+	  <param name="frame_id"                 type="string" value="base"/>
+	  <param name="rgbd_cameras"             type="int"    value="0"/>
+	  <param name="wait_for_imu_to_init"     type="bool"   value="true"/>
+	  
+	  <param name="Odom/ImageDecimation"     type="int"    value="2"/>
+    </node>
+
+    <!-- Visual SLAM -->
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
+      <remap from="imu" to="/trunk_imu"/>
+      <param name="subscribe_depth"  type="bool"   value="false"/>
+      <param name="subscribe_rgbd"   type="bool"   value="true"/>
+      <param name="rgbd_cameras"     type="int"    value="0"/>
+	  <param name="frame_id"         type="string" value="base"/>
+	  
+	  <param name="Grid/RangeMin"    type="string" value="0.1"/> <!-- to avoid adding legs as obstacle in occupancy grid map -->
+	  <param name="Grid/MaxObstacleHeight" type="string" value="1"/>
+	  <param name="Mem/ImagePreDecimation" type="string" value="2"/>
+	  <param name="Mem/ImagePostDecimation" type="string" value="2"/>
+    </node>
+
+    <!-- Visualisation RTAB-Map -->
+    <node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
+      <param name="subscribe_depth"  type="bool"   value="false"/>
+      <param name="subscribe_rgbd"   type="bool"   value="true"/>
+      <param name="subscribe_odom_info" type="bool"   value="true"/>
+      <param name="frame_id"            type="string" value="base"/>
+      <param name="rgbd_cameras"        type="int"    value="0"/>
+    </node>
+
+  </group>
+
+
+</launch>

rtabmap_odom/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(rtabmap_conversions REQUIRED)
 find_package(rtabmap_msgs REQUIRED)
 find_package(rtabmap_util REQUIRED)
+find_package(rtabmap_sync REQUIRED)
 
 include_directories(
   ${CMAKE_CURRENT_SOURCE_DIR}/include
@@ -43,6 +44,7 @@ SET(Libraries
    rtabmap_conversions
    rtabmap_msgs
    rtabmap_util
+   rtabmap_sync
 )
 
 ###########

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
+
 #include <std_srvs/srv/empty.hpp>
 #include <std_msgs/msg/header.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -49,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <boost/thread.hpp>
 
 #include "rtabmap_util/ULogToRosout.h"
+#include "rtabmap_sync/SyncDiagnostic.h"
 
 namespace rtabmap {
 class Odometry;
@@ -56,7 +59,7 @@ class Odometry;
 
 namespace rtabmap_odom {
 
-class OdometryROS : public rclcpp::Node
+class OdometryROS : public rclcpp::Node, public rtabmap_sync::SyncDiagnostic
 {
 
 public:
@@ -83,9 +86,8 @@ public:
 
 protected:
 	void init(bool stereoParams, bool visParams, bool icpParams);
-	void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
-	void callbackCalled() {callbackCalled_ = true;}
 	rmw_qos_reliability_policy_t qos() const {return qos_;}
+	void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
 
 	virtual void flushCallbacks() {};
 	tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
@@ -95,7 +97,6 @@ protected:
 	virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
 
 private:
-	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
 
 	virtual void updateParameters(rtabmap::ParametersMap &) {}
 	virtual void onOdomInit() {}
@@ -105,9 +106,6 @@ private:
 
 private:
 	rtabmap::Odometry * odometry_;
-	std::thread * warningThread_;
-	std::string subscribedTopicsMsg_;
-	bool callbackCalled_;
 
 	// parameters
 	std::string frameId_;
@@ -167,6 +165,17 @@ private:
 	rtabmap::Transform initialPose_;
 
 	rtabmap_util::ULogToRosout ulogToRosout_;
+
+	class OdomStatusTask : public diagnostic_updater::DiagnosticTask
+	{
+	public:
+		OdomStatusTask();
+		void setStatus(bool isLost);
+		void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
+	private:
+		bool lost_;
+	};
+	OdomStatusTask statusDiagnostic_;
 };
 
 }

rtabmap_odom/package.xml

@@ -26,7 +26,8 @@
   <depend>rtabmap_conversions</depend>
   <depend>rtabmap_msgs</depend>
   <depend>rtabmap_util</depend>
-  
+  <depend>rtabmap_sync</depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
   </export>

rtabmap_odom/src/OdometryROS.cpp

@@ -62,9 +62,8 @@ OdometryROS::OdometryROS(const rclcpp::NodeOptions & options) :
 
 OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & options) :
 	Node(name, options),
+	rtabmap_sync::SyncDiagnostic(this, 0.5),
 	odometry_(0),
-	warningThread_(0),
-	callbackCalled_(false),
 	frameId_("base_link"),
 	odomFrameId_("odom"),
 	groundTruthFrameId_(""),
@@ -190,13 +189,6 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 
 OdometryROS::~OdometryROS()
 {
-	if(warningThread_)
-	{
-		callbackCalled();
-		warningThread_->join();
-		delete warningThread_;
-	}
-
 	delete odometry_;
 }
 
@@ -369,28 +361,21 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
 	onOdomInit();
 }
 
-void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
+void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic)
 {
 	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 
-	subscribedTopicsMsg_ = subscribedTopicsMsg;
+	std::vector<diagnostic_updater::DiagnosticTask*> tasks;
-	warningThread_ = new std::thread([&](){
+	tasks.push_back(&statusDiagnostic_);
-		rclcpp::Rate r(1.0/5.0);
+	initDiagnostic(subscribedTopic,
-		while(!callbackCalled_)
+		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 "
-			r.sleep();
+					"header are set. %s%s",
-			if(!callbackCalled_)
+					this->get_name(),
-			{
+					approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
-				RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"topics should have all the exact timestamp for the callback to be called.",
-						"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
+					subscribedTopicsMsg.c_str()),
-						"header are set. %s%s",
+		tasks);
-						this->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());
-			}
-		}
-	});
 }
 
 rtabmap::Transform OdometryROS::velocityGuess() const
@@ -945,6 +930,17 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 		{
 			RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
 		}
+
+		statusDiagnostic_.setStatus(pose.isNull());
+		if(!pose.isNull())
+		{
+			double curentRate = 1.0/(this->now()-timeStart).seconds();
+			tick(header.stamp,
+				maxUpdateRate_>0 && maxUpdateRate_ < curentRate ? maxUpdateRate_:
+				expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
+				previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
+		}
+		
 		previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
 	}
 }
@@ -1046,6 +1042,27 @@ void OdometryROS::setLogError(
 	ULogger::setLevel(ULogger::kError);
 }
 
+OdometryROS::OdomStatusTask::OdomStatusTask() :
+		diagnostic_updater::DiagnosticTask("Odom status"),
+		lost_(false)
+{}
+
+void OdometryROS::OdomStatusTask::setStatus(bool isLost)
+{
+	lost_ = isLost;
+}
+
+void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+	if(lost_)
+	{
+		stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Lost!");
+	}
+	else
+	{
+		stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Tracking.");
+	}
+}
 
 }
 

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -101,6 +101,11 @@ void ICPOdometry::onOdomInit()
 	cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
 
 	filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
+
+	initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
+			get_name(),
+			scan_sub_->get_topic_name(),
+			cloud_sub_->get_topic_name()), true);
 }
 
 void ICPOdometry::updateParameters(ParametersMap & parameters)

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -109,6 +109,7 @@ void RGBDOdometry::onOdomInit()
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color     = %s", keepColor_?"true":"false");
 
+	std::string subscribedTopic;
 	std::string subscribedTopicsMsg;
 	if(subscribeRGBD)
 	{
@@ -260,6 +261,7 @@ void RGBDOdometry::onOdomInit()
 		{
 			rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
 
+			subscribedTopic = rgbdxSub_->get_topic_name();
 			subscribedTopicsMsg = uFormat("\n%s subscribed to:\n   %s",
 					get_name(),
 					rgbdxSub_->get_topic_name());
@@ -268,6 +270,7 @@ void RGBDOdometry::onOdomInit()
 		{
 			rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
 
+			subscribedTopic = rgbdSub_->get_topic_name();
 			subscribedTopicsMsg =
 					uFormat("\n%s subscribed to:\n   %s",
 					get_name(),
@@ -294,6 +297,7 @@ void RGBDOdometry::onOdomInit()
 			exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 		}
 
+		subscribedTopic = image_mono_sub_.getSubscriber().getTopic();
 		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
 				get_name(),
 				approxSync?"approx":"exact",
@@ -302,7 +306,7 @@ void RGBDOdometry::onOdomInit()
 				image_depth_sub_.getSubscriber().getTopic().c_str(),
 				info_sub_.getSubscriber()->get_topic_name());
 	}
-	this->startWarningThread(subscribedTopicsMsg, approxSync);
+	initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
 }
 
 void RGBDOdometry::updateParameters(ParametersMap & parameters)
@@ -492,7 +496,6 @@ void RGBDOdometry::callback(
 		const sensor_msgs::msg::Image::ConstSharedPtr depth,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -521,7 +524,6 @@ void RGBDOdometry::callback(
 void RGBDOdometry::callbackRGBDX(
 		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		if(images->rgbd_images.empty())
@@ -545,7 +547,6 @@ void RGBDOdometry::callbackRGBDX(
 void RGBDOdometry::callbackRGBD(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -562,7 +563,6 @@ void RGBDOdometry::callbackRGBD2(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
@@ -582,7 +582,6 @@ void RGBDOdometry::callbackRGBD3(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
@@ -605,7 +604,6 @@ void RGBDOdometry::callbackRGBD4(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
@@ -631,7 +629,6 @@ void RGBDOdometry::callbackRGBD5(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -90,6 +90,7 @@ void StereoOdometry::onOdomInit()
 	RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color     = %s", keepColor_?"true":"false");
 
+	std::string subscribedTopic;
 	std::string subscribedTopicsMsg;
 	if(subscribeRGBD)
 	{
@@ -206,6 +207,7 @@ void StereoOdometry::onOdomInit()
 		{
 			rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
 
+			subscribedTopic = rgbdxSub_->get_topic_name();
 			subscribedTopicsMsg =
 					uFormat("\n%s subscribed to:\n   %s",
 					get_name(),
@@ -215,6 +217,7 @@ void StereoOdometry::onOdomInit()
 		{
 			rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
 
+			subscribedTopic = rgbdSub_->get_topic_name();
 			subscribedTopicsMsg =
 					uFormat("\n%s subscribed to:\n   %s",
 					get_name(),
@@ -242,6 +245,7 @@ void StereoOdometry::onOdomInit()
 			exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
 		}
 
+		subscribedTopic = imageRectLeft_.getTopic();
 		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
 				get_name(),
 				approxSync?"approx":"exact",
@@ -252,7 +256,7 @@ void StereoOdometry::onOdomInit()
 				cameraInfoRight_.getSubscriber()->get_topic_name());
 	}
 
-	this->startWarningThread(subscribedTopicsMsg, approxSync);
+	initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
 }
 
 void StereoOdometry::updateParameters(ParametersMap & parameters)
@@ -587,7 +591,6 @@ void StereoOdometry::callback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -618,7 +621,6 @@ void StereoOdometry::callback(
 void StereoOdometry::callbackRGBD(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -636,7 +638,6 @@ void StereoOdometry::callbackRGBD(
 void StereoOdometry::callbackRGBDX(
 		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		if(images->rgbd_images.empty())
@@ -663,7 +664,6 @@ void StereoOdometry::callbackRGBD2(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
@@ -686,7 +686,6 @@ void StereoOdometry::callbackRGBD3(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
@@ -713,7 +712,6 @@ void StereoOdometry::callbackRGBD4(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
 {
-	callbackCalled();
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);

rtabmap_slam/src/CoreWrapper.cpp

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

rtabmap_sync/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(rclcpp_components REQUIRED)
 find_package(rtabmap_conversions REQUIRED)
 find_package(rtabmap_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(diagnostic_updater REQUIRED)
 
 option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support"  OFF)
 option(RTABMAP_SYNC_USER_DATA "Build with input user data support"  OFF)
@@ -49,6 +50,7 @@ SET(Libraries
    rtabmap_conversions
    rtabmap_msgs
    sensor_msgs
+   diagnostic_updater
 )
 
 ###########

rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h

@@ -52,10 +52,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_msgs/msg/odom_info.hpp>
 #include <rtabmap_msgs/msg/scan_descriptor.hpp>
 #include <rtabmap_sync/CommonDataSubscriberDefines.h>
+#include <rtabmap_sync/SyncDiagnostic.h>
 
 namespace rtabmap_sync {
 
-class CommonDataSubscriber {
+class CommonDataSubscriber : public SyncDiagnostic {
 public:
 	RTABMAP_SYNC_PUBLIC
 	CommonDataSubscriber(rclcpp::Node & node, bool gui);
@@ -119,7 +120,6 @@ protected:
 				const cv::Mat & localDescriptors = cv::Mat());
 
 private:
-	void callbackCalled() {callbackCalled_ = true;}
 	void setupDepthCallbacks(
 			rclcpp::Node & node,
 			bool subscribeOdom,
@@ -245,8 +245,6 @@ protected:
 
 private:
 	bool approxSync_;
-	std::thread* warningThread_;
-	bool callbackCalled_;
 	bool subscribedToDepth_;
 	bool subscribedToStereo_;
 	bool subscribedToRGB_;

rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h

@@ -0,0 +1,108 @@
+#ifndef INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
+#define INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
+
+#include "rtabmap/utilite/UStl.h"
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <diagnostic_updater/publisher.hpp>
+
+#include "rtabmap_conversions/MsgConversion.h"
+#include "rtabmap/utilite/ULogger.h"
+
+using namespace std::chrono_literals;
+
+namespace rtabmap_sync {
+
+class SyncDiagnostic {
+    public:
+        SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.1, int windowSize = 5) :
+        	node_(node),
+			diagnosticUpdater_(node),
+            frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
+            lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
+            targetFrequency_(0.0),
+            windowSize_(windowSize)
+    {
+        UASSERT(windowSize_ >= 1);
+    }
+
+protected:
+    void initDiagnostic(
+        const std::string & topic,
+        const std::string & topicsNotReceivedWarningMsg,
+        std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
+    {
+        topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg;
+
+        std::list<std::string> strList = uSplit(topic, '/');
+        for(int i=0; i<2 && strList.size()>1; ++i)
+        {
+            // Assuming format is /back_camera/left/image, we want "back_camera"
+            strList.pop_back();
+        }
+        diagnosticUpdater_.add(frequencyStatus_);
+        for(size_t i=0; i<otherTasks.size(); ++i)
+        {
+            diagnosticUpdater_.add(*otherTasks[i]);
+        }
+        diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
+        diagnosticUpdater_.force_update();
+        diagnosticTimer_ = node_->create_wall_timer(1s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
+    }
+
+    void tick(const rclcpp::Time & stamp, double targetFrequency = 0)
+    {
+        frequencyStatus_.tick();
+        double singlePeriod = rtabmap_conversions::timestampFromROS(stamp) - lastCallbackCalledStamp_;
+
+        window_.push_back(singlePeriod);
+        if(window_.size() > windowSize_)
+        {
+            window_.pop_front();
+        }
+        double period = 0.0;
+        if(window_.size() == windowSize_)
+        {
+            for(size_t i=0; i<window_.size(); ++i)
+            {
+                period += window_[i];
+            }
+            period /= windowSize_;
+        }
+
+        if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_))
+        {
+            targetFrequency_ = 1.0/period;
+        }
+        else if(targetFrequency>0)
+        {
+            targetFrequency_ = targetFrequency;
+        }
+        lastCallbackCalledStamp_ = rtabmap_conversions::timestampFromROS(stamp);
+    }
+
+private:
+    void diagnosticTimerCallback()
+    {
+        if(rtabmap_conversions::timestampFromROS(node_->now())-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
+        {
+        	RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, topicsNotReceivedWarningMsg_.c_str());
+        }
+    }
+
+private:
+    rclcpp::Node * node_;
+	std::string topicsNotReceivedWarningMsg_;
+	diagnostic_updater::Updater diagnosticUpdater_;
+	diagnostic_updater::FrequencyStatus frequencyStatus_;
+	rclcpp::TimerBase::SharedPtr diagnosticTimer_;
+	double lastCallbackCalledStamp_;
+	double targetFrequency_;
+	int windowSize_;
+	std::deque<double> window_;
+
+};
+
+}
+
+#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */

rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp

@@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/subscriber.h>
 
 #include "rtabmap_msgs/msg/rgbd_image.hpp"
+#include "rtabmap_sync/SyncDiagnostic.h"
 
 namespace rtabmap_sync
 {
 
-class RGBSync : public rclcpp::Node
+class RGBSync : public rclcpp::Node, public SyncDiagnostic
 {
 public:
 	RTABMAP_SYNC_PUBLIC
@@ -57,13 +58,9 @@ public:
 
 private:
 	double compressedRate_;
-	std::thread * warningThread_;
-	bool callbackCalled_;
 
 	rclcpp::Time lastCompressedPublished_;
 
-	std::string subscribedTopicsMsg_;
-
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
 

rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp

@@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/subscriber.h>
 
 #include "rtabmap_msgs/msg/rgbd_image.hpp"
+#include "rtabmap_sync/SyncDiagnostic.h"
 
 namespace rtabmap_sync
 {
 
-class RGBDSync : public rclcpp::Node
+class RGBDSync : public rclcpp::Node, public SyncDiagnostic
 {
 public:
 	RTABMAP_SYNC_PUBLIC
@@ -60,13 +61,9 @@ private:
 	double depthScale_;
 	int decimation_;
 	double compressedRate_;
-	std::thread * warningThread_;
-	bool callbackCalled_;
 
 	rclcpp::Time lastCompressedPublished_;
 
-	std::string subscribedTopicsMsg_;
-
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
 

rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp

@@ -41,11 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_msgs/msg/rgbd_image.hpp"
 #include "rtabmap_msgs/msg/rgbd_images.hpp"
 #include "rtabmap_sync/CommonDataSubscriber.h"
+#include "rtabmap_sync/SyncDiagnostic.h"
 
 namespace rtabmap_sync
 {
 
-class RGBDXSync : public rclcpp::Node
+class RGBDXSync : public rclcpp::Node, public SyncDiagnostic
 {
 public:
 	RTABMAP_SYNC_PUBLIC
@@ -68,11 +69,6 @@ private:
 	DATA_SYNCS8(rgbd8, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
 
 private:
-	std::thread * warningThread_;
-	bool callbackCalled_;
-
-	std::string subscribedTopicsMsg_;
-
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdImagesPub_;
 
 	std::vector<message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>*> rgbdSubs_;

rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp

@@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/subscriber.h>
 
 #include "rtabmap_msgs/msg/rgbd_image.hpp"
+#include "rtabmap_sync/SyncDiagnostic.h"
 
 namespace rtabmap_sync
 {
 
-class StereoSync : public rclcpp::Node
+class StereoSync : public rclcpp::Node, public SyncDiagnostic
 {
 public:
 	RTABMAP_SYNC_PUBLIC
@@ -58,9 +59,6 @@ public:
 			  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight);
 private:
 	double compressedRate_;
-	std::thread * warningThread_;
-	std::string subscribedTopicsMsg_;
-	bool callbackCalled_;
 	rclcpp::Time lastCompressedPublished_;
 
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;

rtabmap_sync/package.xml

@@ -21,7 +21,8 @@
   <depend>rtabmap_conversions</depend>
   <depend>rtabmap_msgs</depend>
   <depend>sensor_msgs</depend>
-  
+  <depend>diagnostic_updater</depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
   </export>

rtabmap_sync/src/CommonDataSubscriber.cpp

@@ -31,10 +31,9 @@ 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),
-		warningThread_(0),
-		callbackCalled_(false),
 		subscribedToDepth_(!gui),
 		subscribedToStereo_(false),
 		subscribedToRGB_(!gui),
@@ -510,21 +509,8 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 	}
 	else if(subscribedToRGBD_)
 	{
-		if(rgbdCameras_ == 0)
-		{
-			setupRGBDXCallbacks(
-					node,
-					subscribedToOdom_,
-					subscribedToUserData_,
-					subscribedToScan2d_,
-					subscribedToScan3d_,
-					subscribedToScanDescriptor_,
-					subscribedToOdomInfo_,
-					queueSize_,
-					approxSync_);
-		}
 #ifdef RTABMAP_SYNC_MULTI_RGBD
-		else if(rgbdCameras_ >= 6)
+		if(rgbdCameras_ >= 6)
 		{
 			if(rgbdCameras_ > 6)
 			{
@@ -605,6 +591,19 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 					"but you will have to synchronize RGBDImage topics yourself.");
 		}
 #endif
+		else if(rgbdCameras_ == 0)
+		{
+			setupRGBDXCallbacks(
+					node,
+					subscribedToOdom_,
+					subscribedToUserData_,
+					subscribedToScan2d_,
+					subscribedToScan3d_,
+					subscribedToScanDescriptor_,
+					subscribedToOdomInfo_,
+					queueSize_,
+					approxSync_);
+		}
 		else
 		{
 			setupRGBDCallbacks(
@@ -643,40 +642,22 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
 
 	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
 	{
-		warningThread_ = new std::thread([&](){
-			rclcpp::Rate r(1/5.0);
-			while(!callbackCalled_)
-			{
-				r.sleep();
-				if(!callbackCalled_)
-				{
-					RCLCPP_WARN(node.get_logger(),
-							"%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 "
-							"the clocks of the computers are synchronized (\"ntpdate\"). If topics are "
-							"not published at the same rate, you could increase \"queue_size\" parameter "
-							"(current=%d). %s%s",
-							name_.c_str(),
-							queueSize_,
-							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());
-				}
-			}
-		});
 		RCLCPP_INFO(node.get_logger(), "%s", subscribedTopicsMsg_.c_str());
+		initDiagnostic("",
+			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 "
+					"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
+					name_.c_str(),
+					approxSync_?
+							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
+							"Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.",
+					subscribedTopicsMsg_.c_str()));
 	}
 }
 
 CommonDataSubscriber::~CommonDataSubscriber()
 {
-	if(warningThread_)
-	{
-		callbackCalled();
-		warningThread_->join();
-		delete warningThread_;
-	}
-
 	// RGB + Depth
 	SYNC_DEL(depth);
 	SYNC_DEL(depthScan2d);
@@ -1007,8 +988,6 @@ void CommonDataSubscriber::commonSingleCameraCallback(
 		const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d,
 		const cv::Mat & localDescriptors)
 {
-	callbackCalled();
-
 	std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPointsMsgs;
 	localKeyPointsMsgs.push_back(localKeyPoints);
 	std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3dMsgs;

rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp

@@ -32,7 +32,6 @@ namespace rtabmap_sync {
 void CommonDataSubscriber::odomCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
@@ -42,7 +41,6 @@ void CommonDataSubscriber::odomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
@@ -52,7 +50,6 @@ void CommonDataSubscriber::odomDataCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
 }
@@ -61,7 +58,6 @@ void CommonDataSubscriber::odomDataInfoCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	callbackCalled();
 	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
 }

rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp

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

rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp

@@ -32,9 +32,9 @@ namespace rtabmap_sync {
 // Stereo
 void CommonDataSubscriber::stereoCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
-	   const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
-	   const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
-	   const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
 {
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
@@ -45,12 +45,11 @@ void CommonDataSubscriber::stereoCallback(
 }
 void CommonDataSubscriber::stereoInfoCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
-	   const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
-	   const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
-	   const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
-	   const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	callbackCalled();
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
@@ -66,7 +65,6 @@ void CommonDataSubscriber::stereoOdomCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scanMsg; // null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
@@ -81,7 +79,6 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	callbackCalled();
 	rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
 	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null

rtabmap_sync/src/nodelets/rgb_sync.cpp

@@ -43,9 +43,8 @@ namespace rtabmap_sync
 
 RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
 	Node("rgbd_sync", options),
+	SyncDiagnostic(this),
 	compressedRate_(0),
-	warningThread_(0),
-	callbackCalled_(false),
 	approxSync_(0),
 	exactSync_(0)
 {
@@ -88,33 +87,23 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
 	imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
 	cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
 
-	subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s",
+	std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s",
 						get_name(),
 						approxSync?"approx":"exact",
 						approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
 						imageSub_.getSubscriber().getTopic().c_str(),
 						cameraInfoSub_.getSubscriber()->get_topic_name());
 
-	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 
-	warningThread_ = new std::thread([&](){
+	initDiagnostic(imageSub_.getSubscriber().getTopic(),
-		rclcpp::Rate r(1/5.0);
+			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-		while(!callbackCalled_)
+					"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
-		{
+					"header are set. %s%s",
-			r.sleep();
+					this->get_name(),
-			if(!callbackCalled_)
+					approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
-			{
+						"topics should have all the exact timestamp for the callback to be called.",
-				RCLCPP_WARN(this->get_logger(),
+					subscribedTopicsMsg.c_str()));
-						"%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",
-						this->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());
-			}
-		}
-	});
 }
 
 
@@ -124,20 +113,13 @@ RGBSync::~RGBSync()
 		delete approxSync_;
 	if(exactSync_)
 		delete exactSync_;
-
-	if(warningThread_)
-	{
-		callbackCalled_=true;
-		warningThread_->join();
-		delete warningThread_;
-	}
 }
 
 void RGBSync::callback(
 		  const sensor_msgs::msg::Image::ConstSharedPtr image,
 		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
-	callbackCalled_ = true;
+	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,11 +43,10 @@ namespace rtabmap_sync
 
 RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	Node("rgbd_sync", options),
+	SyncDiagnostic(this),
 	depthScale_(1.0),
 	decimation_(1),
 	compressedRate_(0),
-	warningThread_(0),
-	callbackCalled_(false),
 	approxSyncDepth_(0),
 	exactSyncDepth_(0)
 {
@@ -100,7 +99,7 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
 	cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
 
-	subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
+	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():"",
@@ -108,35 +107,22 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 						imageDepthSub_.getSubscriber().getTopic().c_str(),
 						cameraInfoSub_.getSubscriber()->get_topic_name());
 
-	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 
-	warningThread_ = new std::thread([&](){
+	initDiagnostic(imageSub_.getSubscriber().getTopic(),
-		rclcpp::Rate r(1/5.0);
+				uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-		while(!callbackCalled_)
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				RCLCPP_WARN(this->get_logger(),
-						"%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",
-						this->get_name(),
+						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()));
-			}
-		}
-	});
 }
 
 RGBDSync::~RGBDSync()
 {
 	delete approxSyncDepth_;
 	delete exactSyncDepth_;
-	callbackCalled_ = true;
-	warningThread_->join();
-	delete warningThread_;
 }
 
 void RGBDSync::callback(
@@ -144,7 +130,7 @@ void RGBDSync::callback(
 		  const sensor_msgs::msg::Image::ConstSharedPtr depth,
 		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
-	callbackCalled_ = true;
+	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,15 +34,14 @@ namespace rtabmap_sync
 
 RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
 	Node("rgbd_sync", options),
+	SyncDiagnostic(this),
 	SYNC_INIT(rgbd2),
 	SYNC_INIT(rgbd3),
 	SYNC_INIT(rgbd4),
 	SYNC_INIT(rgbd5),
 	SYNC_INIT(rgbd6),
 	SYNC_INIT(rgbd7),
-	SYNC_INIT(rgbd8),
+	SYNC_INIT(rgbd8)
-	warningThread_(0),
-	callbackCalled_(false)
 {
 	int queueSize = 10;
 	bool approxSync = true;
@@ -135,24 +134,15 @@ 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():"");
 
-	warningThread_ = new std::thread([&](){
+	// Setup diagnostic
-		rclcpp::Rate r(1/5.0);
+	initDiagnostic("",
-		while(!callbackCalled_)
+		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 "
-			r.sleep();
+				"header are set. %s%s",
-			if(!callbackCalled_)
+				get_name(),
-			{
+				approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
-				RCLCPP_WARN(this->get_logger(),
+					"topics should have all the exact timestamp for the callback to be called.",
-						"%s: Did not receive data since 5 seconds! Make sure the input topics are "
+					subscribedTopicsMsg_.c_str()));
-						"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
-						"header are set. %s%s",
-						this->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());
-			}
-		}
-	});
 }
 
 RGBDXSync::~RGBDXSync()
@@ -164,20 +154,13 @@ RGBDXSync::~RGBDXSync()
 	SYNC_DEL(rgbd6);
 	SYNC_DEL(rgbd7);
 	SYNC_DEL(rgbd8);
-
-	if(warningThread_)
-	{
-		callbackCalled_=true;
-		warningThread_->join();
-		delete warningThread_;
-	}
 }
 
 void RGBDXSync::rgbd2Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(2);
@@ -191,7 +174,7 @@ void RGBDXSync::rgbd3Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(3);
@@ -207,7 +190,7 @@ void RGBDXSync::rgbd4Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(4);
@@ -225,7 +208,7 @@ void RGBDXSync::rgbd5Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(5);
@@ -245,7 +228,7 @@ void RGBDXSync::rgbd6Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(6);
@@ -267,7 +250,7 @@ void RGBDXSync::rgbd7Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
 {
-	callbackCalled_ = true;
+	tick(image0->header.stamp);
 	rtabmap_msgs::msg::RGBDImages output;
 	output.header = image0->header;
 	output.rgbd_images.resize(7);
@@ -291,7 +274,7 @@ void RGBDXSync::rgbd8Callback(
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6,
 		  const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7)
 {
-	callbackCalled_ = true;
+	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,9 +42,8 @@ namespace rtabmap_sync
 
 StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 		Node("stereo_sync", options),
+		SyncDiagnostic(this),
 		compressedRate_(0),
-		warningThread_(0),
-		callbackCalled_(false),
 		approxSync_(0),
 		exactSync_(0)
 {
@@ -88,7 +87,7 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 	cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
 	cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
 
-	subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s,\n   %s",
+	std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s,\n   %s",
 						get_name(),
 						approxSync?"approx":"exact",
 						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
@@ -97,36 +96,22 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 						cameraInfoLeftSub_.getSubscriber()->get_topic_name(),
 						cameraInfoRightSub_.getSubscriber()->get_topic_name());
 
-	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 
-	warningThread_ = new std::thread([&](){
+	initDiagnostic(imageLeftSub_.getSubscriber().getTopic(),
-		rclcpp::Rate r(1/5.0);
+		uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-		while(!callbackCalled_)
+				"published (\"$ rostopic hz my_topic\") and the timestamps in their "
-		{
+				"header are set. %s%s",
-			r.sleep();
+				get_name(),
-			if(!callbackCalled_)
+				approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
-			{
+					"topics should have all the exact timestamp for the callback to be called.",
-				RCLCPP_WARN(this->get_logger(),
+				subscribedTopicsMsg.c_str()));
-						"%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",
-						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());
-			}
-		}
-	});
 }
 
 StereoSync::~StereoSync()
 {
 	delete approxSync_;
 	delete exactSync_;
-
-	callbackCalled_=true;
-	warningThread_->join();
-	delete warningThread_;
 }
 
 void StereoSync::callback(
@@ -135,7 +120,7 @@ void StereoSync::callback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
 {
-	callbackCalled_ = true;
+	tick(imageLeft->header.stamp);
 	if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
 	{
 		double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp);

rtabmap_viz/src/GuiWrapper.cpp

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