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

Stale inputs detection (#1358)

Browse Source 
* Added stale_update_detection parameter to detect if upstream is stale for too long, trigger new map

* On too small guess motion, still publish odom topic along tf

* Added parameters when guess odom topic is sent

* fixed stale disabled logic

* Added info when odometry update is skipped.

* Making vo publishing the expected output data even if odometry update was skip by not enough motion

* fixed skipping odom

* refactored tooOldPreviousData to work with guess

* renamed stale_update_detection to staleness_factor

* cleanup

* reverting a change

---------

Co-authored-by: mathieu86 <[email protected]>
This commit is contained in:
matlabbe
2025-09-23 14:10:57 -07:00
committed by GitHub
co-authored by mathieu86
parent 7d4de30ec7
commit 46f4443baa
4 changed files with 132 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/src/CoreWrapper.cpp
+50
View File
@@ -104,6 +104,7 @@ CoreWrapper::CoreWrapper() :
landmarkDefaultLinVariance_(0.001),
waitForTransform_(true),
waitForTransformDuration_(0.2), // 200 ms
stalenessFactor_(0.0),
useActionForGoal_(false),
useSavedMap_(true),
genScan_(false),
@@ -197,6 +198,7 @@ void CoreWrapper::onInit()
pnh.param("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_,pubLocPoseOnlyWhenLocalizing_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("staleness_factor", stalenessFactor_, stalenessFactor_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
@@ -248,6 +250,14 @@ void CoreWrapper::onInit()
NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false");
NODELET_INFO("rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false");
NODELET_INFO("rtabmap: wait_for_transform = %s", waitForTransform_?"true":"false");
NODELET_INFO("rtabmap: wait_for_transform_duration = %f", waitForTransformDuration_);
if(stalenessFactor_!=0.0 && stalenessFactor_ < 1.0) {
NODELET_ERROR("rtabmap: staleness_factor should be 0 (disabled) or >= 1 (value that multiplies the detection update period). Current value is %f, setting it to 0...",
stalenessFactor_);
stalenessFactor_ = 0.0;
}
NODELET_INFO("rtabmap: staleness_factor = %f", stalenessFactor_);
bool subscribeStereo = false;
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
if(subscribeStereo)
@@ -1054,6 +1064,26 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti
UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0]));
rtabmap_.triggerNewMap();
covariance_ = cv::Mat();
}
else if(stalenessFactor_>0.0 &&
previousStamp_.toSec() > 0.0 &&
rate_>0.0f &&
stamp.toSec() - previousStamp_.toSec() > stalenessFactor_/rate_)
{
UWARN("The time difference (%f s) between the new timestamp received (%f) and "
"the previous one (%f) is way over than the expected update period (%s=%f Hz) "
"%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 "
"to avoid triggering a new map when this happens.",
stamp.toSec() - previousStamp_.toSec(),
stamp.toSec(),
previousStamp_.toSec(),
Parameters::kRtabmapDetectionRate().c_str(),
rate_,
1.0f/rate_,
stalenessFactor_,
stalenessFactor_/rate_);
rtabmap_.triggerNewMap();
covariance_ = cv::Mat();
}
lastPoseIntermediate_ = false;
@@ -1147,6 +1177,26 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
rtabmap_.triggerNewMap();
covariance_ = cv::Mat();
}
else if(stalenessFactor_>0.0 &&
previousStamp_.toSec() > 0.0 &&
rate_>0.0f &&
stamp.toSec() - previousStamp_.toSec() > stalenessFactor_/rate_)
{
UWARN("The time difference (%f s) between the new timestamp received (%f) and "
"the previous one (%f) is way over than the expected update period (%s=%f Hz) "
"%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 "
"to avoid triggering a new map when this happens.",
stamp.toSec() - previousStamp_.toSec(),
stamp.toSec(),
previousStamp_.toSec(),
Parameters::kRtabmapDetectionRate().c_str(),
rate_,
1.0f/rate_,
stalenessFactor_,
stalenessFactor_/rate_);
rtabmap_.triggerNewMap();
covariance_ = cv::Mat();
}
lastPoseIntermediate_ = false;
lastPose_ = odom;