From 0ba59996e695ade7b20a182fbce0bc447537cc86 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Jul 2025 22:01:22 +0000 Subject: [PATCH] refactored time jump detection --- rtabmap_odom/src/OdometryROS.cpp | 1 - rtabmap_slam/src/CoreWrapper.cpp | 18 ------------------ 2 files changed, 19 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index f5b54ba9..81d08076 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -1202,7 +1202,6 @@ void OdometryROS::reset(const Transform & pose) imus_.clear(); imuMutex_.unlock(); this->flushCallbacks(); - this->tfListener().clear(); } bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index ff930a44..6a3ab207 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1023,15 +1023,6 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti { if(!paused_) { - // Check time jump in the past - if(stamp < previousStamp_) { - ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", - previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); - previousStamp_ = ros::Time(); - tfListener_.clear(); - return false; - } - Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); if(!odom.isNull()) { @@ -1143,15 +1134,6 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) { if(!paused_) { - // Check time jump in the past - if(stamp < previousStamp_) { - ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", - previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); - previousStamp_ = ros::Time(); - tfListener_.clear(); - return false; - } - // Odom TF ready? Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); if(odom.isNull())