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

Fixed vo reset from guess (after being lost) not correctly updated if it is still lost after auto reset countdown

Browse Source
This commit is contained in:
matlabbe
2024-12-13 14:33:35 -08:00
parent 01b8ab80ee
commit ef8ec4357d
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/OdometryROS.cpp
+6 -2
View File
@@ -905,10 +905,9 @@ void OdometryROS::mainLoop()
"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec()); header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec());
} }
else else if(--resetCurrentCount_>0)
{ {
NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
--resetCurrentCount_;
} }
if(resetCurrentCount_ == 0 || tooOldPreviousData) if(resetCurrentCount_ == 0 || tooOldPreviousData)
@@ -936,6 +935,11 @@ void OdometryROS::mainLoop()
odometry_->reset(tfPose); odometry_->reset(tfPose);
} }
} }
// Keep resetting if the odometry cannot initialize in next updates (e.g., lack of features).
// This will make sure we keep updating to latest guess pose.
if(resetCurrentCount_ == 0) {
++resetCurrentCount_;
}
} }
} }