From 9a4c585d5ef406d222e727645095a764163d8ca1 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 7 Dec 2024 17:26:46 -0800
Subject: [PATCH 1/2] Fixed https://github.com/introlab/rtabmap/issues/1399

---
 .../include/rtabmap_odom/OdometryROS.h        |  1 +
 rtabmap_odom/src/OdometryROS.cpp              | 35 ++++++++++++++-----
 2 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
index fe632fd4..c5672d36 100644
--- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
@@ -148,6 +148,7 @@ private:
 	USemaphore dataReady_;
 	rtabmap::SensorData dataToProcess_;
 	std_msgs::Header dataHeaderToProcess_;
+	bool bufferedDataToProcess_;
 
 	bool paused_;
 	int resetCountdown_;
diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp
index a46d4fb2..a75ab336 100644
--- a/rtabmap_odom/src/OdometryROS.cpp
+++ b/rtabmap_odom/src/OdometryROS.cpp
@@ -436,13 +436,24 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 				cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
 				localTransform);
 
-		UScopeMutex m(imuMutex_);
-
-		imus_.insert(std::make_pair(stamp, imu));
-		if(imus_.size() > 1000)
 		{
-			NODELET_WARN("Dropping imu data!");
-			imus_.erase(imus_.begin());
+			UScopeMutex m(imuMutex_);
+
+			imus_.insert(std::make_pair(stamp, imu));
+			if(imus_.size() > 1000)
+			{
+				NODELET_WARN("Dropping imu data!");
+				imus_.erase(imus_.begin());
+			}
+		}
+		if(dataMutex_.lockTry() == 0)
+		{
+			if(bufferedDataToProcess_ && dataHeaderToProcess_.stamp.toSec() <= stamp)
+			{
+				bufferedDataToProcess_ = false;
+				dataReady_.release();
+			}
+			dataMutex_.unlock();
 		}
 	}
 }
@@ -454,6 +465,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 	{
 		dataToProcess_ = data;
 		dataHeaderToProcess_ = header;
+		bufferedDataToProcess_ = false;
 		dataReady_.release();
 		dataMutex_.unlock();
 	}
@@ -497,8 +509,15 @@ void OdometryROS::mainLoop()
 
 		if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec()))
 		{
-			NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)",
-					data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
+			if(bufferedDataToProcess_) {
+				NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Previous image is dropped, buffering the new image until an imu with same or greater stamp is received.",
+						data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
+			}
+			else {
+				NODELET_WARN("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);
+				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)

From 01b8ab80ee122bca574269c97fc73994c8cdec59 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 7 Dec 2024 17:47:36 -0800
Subject: [PATCH 2/2] refactored last commit

---
 rtabmap_odom/src/OdometryROS.cpp | 17 ++++++++---------
 1 file changed, 8 insertions(+), 9 deletions(-)

diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp
index a75ab336..0155b86e 100644
--- a/rtabmap_odom/src/OdometryROS.cpp
+++ b/rtabmap_odom/src/OdometryROS.cpp
@@ -463,6 +463,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 	//NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec());
 	if(dataMutex_.lockTry() == 0)
 	{
+		if(bufferedDataToProcess_) {
+			NODELET_ERROR("We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!",
+						dataHeaderToProcess_.stamp.toSec(), header.stamp.toSec());
+		}
 		dataToProcess_ = data;
 		dataHeaderToProcess_ = header;
 		bufferedDataToProcess_ = false;
@@ -509,15 +513,9 @@ void OdometryROS::mainLoop()
 
 		if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec()))
 		{
-			if(bufferedDataToProcess_) {
-				NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Previous image is dropped, buffering the new image until an imu with same or greater stamp is received.",
-						data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
-			}
-			else {
-				NODELET_WARN("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);
-				bufferedDataToProcess_ = true;
-			}
+			NODELET_WARN("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);
+			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)
@@ -1128,6 +1126,7 @@ void OdometryROS::reset(const Transform & pose)
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();
 	dataHeaderToProcess_ = std_msgs::Header();
+	bufferedDataToProcess_ = false;
 	imuMutex_.lock();
 	imus_.clear();
 	imuMutex_.unlock();