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_odom/include/rtabmap_odom/OdometryROS.h

@@ -98,7 +98,7 @@ protected:
 	virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
 
 private:
-
+	void processData();
 	virtual void mainLoop();
 	virtual void mainLoopKill();
 	virtual void updateParameters(rtabmap::ParametersMap &) {}
@@ -174,9 +174,12 @@ private:
 	rtabmap::Transform guessPreviousPose_;
 	double previousStamp_;
 	double previousClockTime_;
+	double lastReceivedTopicClock_;
+	double lastReceivedTopicStamp_;
 	double expectedUpdateRate_;
 	double maxUpdateRate_;
 	double minUpdateRate_;
+	bool alwaysProcessMostRecentFrame_;
 	std::string compressionImgFormat_;
 	bool compressionParallelized_;
 	int odomStrategy_;

rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp

@@ -78,6 +78,7 @@ private:
 	double scanNormalGroundUp_;
 	bool deskewing_;
 	bool deskewingSlerp_;
+	int topicQueueSize_;
 	//std::vector<std::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
 	//pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
 	bool scanReceived_ = false;

rtabmap_odom/src/OdometryROS.cpp

@@ -89,9 +89,12 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	icpParams_(false),
 	previousStamp_(0.0),
 	previousClockTime_(0.0),
+	lastReceivedTopicClock_(0.0),
+	lastReceivedTopicStamp_(0.0),
 	expectedUpdateRate_(0.0),
 	maxUpdateRate_(0.0),
 	minUpdateRate_(0.0),
+	alwaysProcessMostRecentFrame_(true),
 	compressionImgFormat_(".jpg"),
 	compressionParallelized_(true),
 	odomStrategy_(Parameters::defaultOdomStrategy()),
@@ -144,6 +147,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_);
 	maxUpdateRate_ = this->declare_parameter("max_update_rate", maxUpdateRate_);
 	minUpdateRate_ = this->declare_parameter("min_update_rate", minUpdateRate_);
+	alwaysProcessMostRecentFrame_ = this->declare_parameter("always_process_most_recent_frame", alwaysProcessMostRecentFrame_);
 
 	compressionImgFormat_ = this->declare_parameter("sensor_data_compression_format", compressionImgFormat_);
 	compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_);
@@ -461,25 +465,47 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
 void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header)
 {
 	//RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds());
+	double clockNow = rtabmap_conversions::timestampFromROS(now());
 	if(dataMutex_.lockTry() == 0)
 	{
 		if(bufferedDataToProcess_) {
-			RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!",
+			RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped!",
 						rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp), rtabmap_conversions::timestampFromROS(header.stamp));
 			++droppedMsgs_;
 		}
 		dataToProcess_ = data;
 		dataHeaderToProcess_ = header;
 		bufferedDataToProcess_ = false;
-		dataReady_.release();
+		if(alwaysProcessMostRecentFrame_) {
+			dataReady_.release();
+		}
 		dataMutex_.unlock();
 		++processedMsgs_;
+		if(!alwaysProcessMostRecentFrame_) {
+			processData();
+		}
 	}
 	else
 	{
-		//RCLCPP_WARN(get_logger(), "Dropping image/scan data");
+		double estimatedPeriod = clockNow - lastReceivedTopicClock_;
+		double topicPeriod = rtabmap_conversions::timestampFromROS(header.stamp) - lastReceivedTopicStamp_;
+		if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.9) {
+			RCLCPP_WARN(get_logger(), 
+			"Dropping image/scan data with stamp %f (delay=%f). Something is wrong "
+			"because the clock difference with the previous topic received (%fs) is much lower than the "
+			"expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing "
+			"a large bag with flaky replaying delay, consider setting parameter \"always_process_most_recent_frame:=false\" "
+			"to avoid aggressively dropping data.",
+			rtabmap_conversions::timestampFromROS(header.stamp),
+			clockNow - rtabmap_conversions::timestampFromROS(header.stamp),
+			estimatedPeriod,
+			topicPeriod,
+			lastReceivedTopicStamp_);
+		}
 		++droppedMsgs_;
 	}
+	lastReceivedTopicStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
+	lastReceivedTopicClock_ = clockNow;
 }
 
 void OdometryROS::mainLoopKill()
@@ -497,7 +523,10 @@ void OdometryROS::mainLoop()
 		// thread killed
 		return;
 	}
-
+	processData();
+}
+void OdometryROS::processData()
+{
 	UScopeMutex lock(dataMutex_);
 
 	// aliases
@@ -516,21 +545,37 @@ void OdometryROS::mainLoop()
 
 		if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
 		{
-			RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.",
-					data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
+			if(!imus_.empty()) {
+				RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image/scan stamp=%f and last imu stamp received=%f). Buffering the image/scan until an imu with same or greater stamp is received.",
+						data.stamp(), imus_.rbegin()->first);
+			}
+			else {
+				// If empty, it is an error!
+				RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image/scan stamp=%f and imu buffer is empty). Buffering the image/scan until an imu with same or greater stamp is received.",
+						data.stamp());
+			}
 			bufferedDataToProcess_ = true;
 			return;
 		}
 		// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
 		std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
+		std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iterLast = iterEnd;
 		if(iterEnd!= imus_.end())
 		{
 			++iterEnd;
 		}
 		for(std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iter=imus_.begin(); iter!=iterEnd;)
 		{
-			imus.push_back(*iter);
-			imus_.erase(iter++);
+			// Because we always keep the last processed imu in the buffer, skip the first one when processing again the buffer.
+			if(iter!=imus_.begin()) {
+				imus.push_back(*iter);
+			}
+			if(iter!=iterLast) {
+				imus_.erase(iter++);
+			}
+			else {
+				++iter;
+			}
 		}
 	} // end imu lock
 
@@ -1241,6 +1286,8 @@ void OdometryROS::reset(const Transform & pose)
 	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	previousClockTime_ = 0.0;
+	lastReceivedTopicClock_ = 0.0;
+	lastReceivedTopicStamp_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -60,6 +60,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
 	scanNormalGroundUp_(0.0),
 	deskewing_(false),
 	deskewingSlerp_(false),
+	topicQueueSize_(1),
 	scanReceived_(false),
 	cloudReceived_(false)
 {
@@ -83,6 +84,7 @@ void ICPOdometry::onOdomInit()
 	scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_);
 	deskewing_ = this->declare_parameter("deskewing", deskewing_);
 	deskewingSlerp_ = this->declare_parameter("deskewing_slerp", deskewingSlerp_);
+	topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_);
 
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos                    = %d", (int)qos());
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points  = %d", scanCloudMaxPoints_);
@@ -96,12 +98,13 @@ void ICPOdometry::onOdomInit()
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up  = %f", scanNormalGroundUp_);
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing              = %s", deskewing_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp        = %s", deskewingSlerp_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: topic_queue_size       = %d", topicQueueSize_);
 
 	rclcpp::SubscriptionOptions options;
 	options.callback_group = dataCallbackGroup_;
 
-	scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
-	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), options);
+	scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
+	cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options);
 
 	filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
 

rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h

@@ -62,7 +62,7 @@ class SyncDiagnostic {
         diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
     }
 
-    void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0)
+    void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0.0)
     {
         updateFrequency(
             stamp,
@@ -74,9 +74,12 @@ class SyncDiagnostic {
             lastTickInputStamp_);
     }
 
-    void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0)
+    void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0.0)
     {
-        double lastTickOutputStamp;
+        if(expectedFrequency == 0.0) {
+            outTargetFrequency_ = inTargetFrequency_;
+        }
+        double lastTickOutputStamp = 0.0;
         updateFrequency(
             stamp,
             expectedFrequency,
@@ -112,31 +115,33 @@ private:
 		timeStatus.tick(stamp);
 
         double stampSec = rtabmap_conversions::timestampFromROS(stamp);
-        double singlePeriod = stampSec - lastTickStamp;
 
-        window.push_back(singlePeriod);
-        if(window.size() > windowSize_)
+        if(expectedFrequency>0)
         {
-            window.pop_front();
+            targetFrequency = expectedFrequency;
+        }
+        else if(lastTickStamp > 0.0) {
+            double singlePeriod = stampSec - lastTickStamp;
 
-             double period = 0.0;
-            if(window.size() == windowSize_)
+            window.push_back(singlePeriod);
+            if(window.size() > windowSize_)
             {
-                for(size_t i=0; i<window.size(); ++i)
+                window.pop_front();
+
+                double period = 0.0;
+                if(window.size() == windowSize_)
                 {
-                    period += window[i];
+                    for(size_t i=0; i<window.size(); ++i)
+                    {
+                        period += window[i];
+                    }
+                    period /= windowSize_;
                 }
-                period /= windowSize_;
-            }
 
-            if(period>0.0 && expectedFrequency == 0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency))
-            {
-                targetFrequency = 1.0/period;
-            }
-            else if(expectedFrequency>0)
-            {
-                targetFrequency = expectedFrequency;
-            
+                if(period>0.0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency))
+                {
+                    targetFrequency = 1.0/period;
+                }
             }
         }
 

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);
 }
 
 }

rtabmap_viz/src/GuiWrapper.cpp

@@ -203,7 +203,11 @@ void GuiWrapper::infoMapCallback(
 
 	this->post(new RtabmapEvent(stat));
 
-	tick(infoMsg->header.stamp);
+	ParametersMap allParameters = prefDialog_->getAllParameters();
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	Parameters::parse(allParameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+
+	tick(infoMsg->header.stamp, detectionRate);
 
 }
 
@@ -236,7 +240,11 @@ void GuiWrapper::infoCallback(
 
 	this->post(new RtabmapEvent(stat));
 
-	tick(infoMsg->header.stamp);
+	ParametersMap allParameters = prefDialog_->getAllParameters();
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	Parameters::parse(allParameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+
+	tick(infoMsg->header.stamp, detectionRate);
 }
 
 void GuiWrapper::goalPathCallback(