Odom: added parameter "always_process_most_recent_frame" (default true like before) so that we can disable aggressive frame dropping in case input frames are published with flaky latency (e.g., with large rosbag having issue to replay in time topics)

rtabmap_util/CMakeLists.txt

@@ -26,6 +26,7 @@ find_package(pcl_ros REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(rtabmap_msgs REQUIRED)
 find_package(rtabmap_conversions REQUIRED)
+find_package(rtabmap_sync REQUIRED)
 
 # Optional components
 find_package(octomap_msgs)
@@ -54,6 +55,7 @@ SET(Libraries
    message_filters
    rtabmap_msgs
    rtabmap_conversions
+   rtabmap_sync
 )
 
 if("$ENV{ROS_DISTRO}" STRLESS "jazzy")

rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp

@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_util/visibility.h>
 #include "rclcpp/rclcpp.hpp"
 
+#include <rtabmap_sync/SyncDiagnostic.h>
+
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
@@ -58,6 +60,8 @@ private:
 	bool slerp_;
 	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
 	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+	std::unique_ptr<rtabmap_sync::SyncDiagnostic> scanSyncDiagnostic_;
+	std::unique_ptr<rtabmap_sync::SyncDiagnostic> cloudSyncDiagnostic_;
 };
 
 }

rtabmap_util/package.xml

@@ -32,6 +32,7 @@
   <depend>message_filters</depend>
   <depend>rtabmap_msgs</depend>
   <depend>rtabmap_conversions</depend>
+  <depend>rtabmap_sync</depend>
   <depend>grid_map_ros</depend>
 
   <export>

rtabmap_util/src/nodelets/lidar_deskewing.cpp

@@ -46,6 +46,16 @@ LidarDeskewing::~LidarDeskewing()
 
 void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
 {
+	if(scanSyncDiagnostic_.get() == 0) {
+		scanSyncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5));
+		scanSyncDiagnostic_->init(subScan_->get_topic_name(),
+			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topic \"%s\" is "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set.",
+						this->get_name(),
+						subScan_->get_topic_name()));
+	}
+	scanSyncDiagnostic_->tickInput(msg->header.stamp);
 	// make sure the frame of the laser is updated during the whole scan time
 	rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform(
 			msg->header.frame_id,
@@ -75,10 +85,23 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared
 	rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
 	scanOutDeskewed.header.frame_id = msg->header.frame_id;
 	pubScan_->publish(scanOutDeskewed);
+
+	scanSyncDiagnostic_->tickOutput(msg->header.stamp);
 }
 
 void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
 {
+	if(cloudSyncDiagnostic_.get() == 0) {
+		cloudSyncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5));
+		cloudSyncDiagnostic_->init(subCloud_->get_topic_name(),
+			uFormat("%s: Did not receive data since 5 seconds! Make sure the input topic \"%s\" is "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set.",
+						this->get_name(),
+						subCloud_->get_topic_name()));
+	}
+	cloudSyncDiagnostic_->tickInput(msg->header.stamp);
+
 	sensor_msgs::msg::PointCloud2 msgDeskewed;
 	if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfBuffer_, waitForTransformDuration_, slerp_))
 	{
@@ -91,6 +114,7 @@ void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSha
 		RCLCPP_WARN(this->get_logger(), "deskewing failed! returning possible skewed cloud!");
 		pubCloud_->publish(*msg);
 	}
+	cloudSyncDiagnostic_->tickOutput(msg->header.stamp);
 }
 
 }