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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2024-12-14 12:01:25 -08:00
parent 633422e65c ef8ec4357d
commit cbd3088eba
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/OdometryROS.cpp
+6 -2
View File
@@ -940,10 +940,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.", "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp)); rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
} }
else else if(--resetCurrentCount_>0)
{ {
RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
--resetCurrentCount_;
} }
if(resetCurrentCount_ == 0 || tooOldPreviousData) if(resetCurrentCount_ == 0 || tooOldPreviousData)
@@ -971,6 +970,11 @@ void OdometryROS::mainLoop()
odometry_->reset(tfPose); 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_;
}
} }
} }