From ef8ec4357d76fc83351acfaff0fc31c778214f46 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 13 Dec 2024 14:33:35 -0800 Subject: [PATCH] Fixed vo reset from guess (after being lost) not correctly updated if it is still lost after auto reset countdown --- rtabmap_odom/src/OdometryROS.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 0155b86e1..f79fca441 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -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.", 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_); - --resetCurrentCount_; } if(resetCurrentCount_ == 0 || tooOldPreviousData) @@ -936,6 +935,11 @@ void OdometryROS::mainLoop() 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_; + } } }