merged master->ros2

introlab · Dec 8, 2024 · 633422e · 633422e
2 parents 42b0b49 + 01b8ab8
commit 633422e
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 14 deletions.
diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
@@ -162,6 +162,7 @@ class OdometryROS : public rclcpp::Node, public UThread
 	USemaphore dataReady_;
 	rtabmap::SensorData dataToProcess_;
 	std_msgs::msg::Header dataHeaderToProcess_;
+	bool bufferedDataToProcess_;
 
 	bool paused_;
 	int resetCountdown_;

diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp
@@ -420,25 +420,36 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
 		double stamp = rtabmap_conversions::timestampFromROS(msg->header.stamp);
 		//RCLCPP_WARN(get_logger(), "Received imu: %f delay=%f", stamp, (now() - msg->header.stamp).seconds());
 
-		UScopeMutex m(imuMutex_);
-
-		if(!imuProcessed_ && imus_.empty())
 		{
-			rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_);
-			if(localTransform.isNull())
+			UScopeMutex m(imuMutex_);
+
+			if(!imuProcessed_ && imus_.empty())
 			{
-				RCLCPP_WARN(this->get_logger(), "Dropping imu data! A valid TF between %s and %s is required to initialize IMU.",
-					this->frameId().c_str(), msg->header.frame_id.c_str());
-				return;
+				rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_);
+				if(localTransform.isNull())
+				{
+					RCLCPP_WARN(this->get_logger(), "Dropping imu data! A valid TF between %s and %s is required to initialize IMU.",
+						this->frameId().c_str(), msg->header.frame_id.c_str());
+					return;
+				}
 			}
-		}
 
-		imus_.insert(std::make_pair(stamp, msg));
+			imus_.insert(std::make_pair(stamp, msg));
 
-		if(imus_.size() > 1000)
+			if(imus_.size() > 1000)
+			{
+				RCLCPP_WARN(this->get_logger(), "Dropping imu data!");
+				imus_.erase(imus_.begin());
+			}
+		}
+		if(dataMutex_.lockTry() == 0)
 		{
-			RCLCPP_WARN(this->get_logger(), "Dropping imu data!");
-			imus_.erase(imus_.begin());
+			if(bufferedDataToProcess_ && rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp) <= stamp)
+			{
+				bufferedDataToProcess_ = false;
+				dataReady_.release();
+			}
+			dataMutex_.unlock();
 		}
 	}
 }
@@ -448,8 +459,14 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 	//RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds());
 	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!",
+						rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp), rtabmap_conversions::timestampFromROS(header.stamp));
+			++droppedMsgs_;
+		}
 		dataToProcess_ = data;
 		dataHeaderToProcess_ = header;
+		bufferedDataToProcess_ = false;
 		dataReady_.release();
 		dataMutex_.unlock();
 		++processedMsgs_;
@@ -495,8 +512,9 @@ void OdometryROS::mainLoop()
 
 		if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
 		{
-			RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)",
+			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);
+			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)
@@ -1149,6 +1167,7 @@ void OdometryROS::reset(const Transform & pose)
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();
 	dataHeaderToProcess_ = std_msgs::msg::Header();
+	bufferedDataToProcess_ = false;
 	imuMutex_.lock();
 	imus_.clear();
 	imuMutex_.unlock();