Fixed introlab/rtabmap#1399

introlab · Dec 8, 2024 · 9a4c585 · 9a4c585
1 parent 3bdbcdf
commit 9a4c585
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 7 deletions.
diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
@@ -148,6 +148,7 @@ class OdometryROS : public nodelet::Nodelet, public UThread
 	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
@@ -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_);
+		{
+			UScopeMutex m(imuMutex_);
 
-		imus_.insert(std::make_pair(stamp, imu));
-		if(imus_.size() > 1000)
+			imus_.insert(std::make_pair(stamp, imu));
+			if(imus_.size() > 1000)
+			{
+				NODELET_WARN("Dropping imu data!");
+				imus_.erase(imus_.begin());
+			}
+		}
+		if(dataMutex_.lockTry() == 0)
 		{
-			NODELET_WARN("Dropping imu data!");
-			imus_.erase(imus_.begin());
+			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)