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

refactored time jump detection

Browse Source
This commit is contained in:
matlabbe
2025-07-04 22:01:22 +00:00
parent 3cc9db8f87
commit 0ba59996e6
2 changed files with 0 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/OdometryROS.cpp
-1
View File
@@ -1202,7 +1202,6 @@ void OdometryROS::reset(const Transform & pose)
imus_.clear(); imus_.clear();
imuMutex_.unlock(); imuMutex_.unlock();
this->flushCallbacks(); this->flushCallbacks();
this->tfListener().clear();
} }
bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
rtabmap_slam/src/CoreWrapper.cpp
-18
View File
@@ -1023,15 +1023,6 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti
{ {
if(!paused_) 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); Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
if(!odom.isNull()) if(!odom.isNull())
{ {
@@ -1143,15 +1134,6 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
{ {
if(!paused_) 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? // Odom TF ready?
Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
if(odom.isNull()) if(odom.isNull())