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

Odom reset on time jump in the past (#1333)

Browse Source 
* Odom reset on time jump

* Adding more logs to debug

* refactored

* dont skip frame on clock jump

* Making clock check independent of the topic stamp check

* fixed post check

* Making diagnostic more robust to time jump

* Added node name to warning

* make sync warning msg working in case of time jump

* not need to reset timer

* reset timer

* timer auto reset already

* typo

* Added more time checks to make sure we don't republish a tf frame with stamp from a topic in the future

* dont send tf if time jump happened while processing

* fixed errors

* addressing comments

* fixing time comparison
This commit is contained in:
matlabbe
2025-07-03 17:50:38 -07:00
committed by GitHub
parent fa342bd853
commit 3cc9db8f87
5 changed files with 136 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/src/CoreWrapper.cpp
+18
View File
@@ -1023,6 +1023,15 @@ 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())
{
@@ -1134,6 +1143,15 @@ 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())