i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactored last commit

Browse Source
This commit is contained in:
matlabbe
2024-12-07 17:47:36 -08:00
parent 9a4c585d5e
commit 01b8ab80ee
1 changed files with 8 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/OdometryROS.cpp
+8 -9
View File
@@ -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()); //NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec());
if(dataMutex_.lockTry() == 0) 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; dataToProcess_ = data;
dataHeaderToProcess_ = header; dataHeaderToProcess_ = header;
bufferedDataToProcess_ = false; bufferedDataToProcess_ = false;
@@ -509,15 +513,9 @@ void OdometryROS::mainLoop()
if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec()))
{ {
if(bufferedDataToProcess_) { 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.",
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);
data.stamp(), imus_.empty()?0:imus_.rbegin()->first); bufferedDataToProcess_ = true;
}
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; 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) // 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; imuProcessed_ = false;
dataToProcess_ = SensorData(); dataToProcess_ = SensorData();
dataHeaderToProcess_ = std_msgs::Header(); dataHeaderToProcess_ = std_msgs::Header();
bufferedDataToProcess_ = false;
imuMutex_.lock(); imuMutex_.lock();
imus_.clear(); imus_.clear();
imuMutex_.unlock(); imuMutex_.unlock();