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

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_sync/include/rtabmap_sync/SyncDiagnostic.h
+22 -5
View File
@@ -19,7 +19,9 @@ class SyncDiagnostic {
compositeTask_("Sync status"),
lastCallbackCalledStamp_(ros::Time::now().toSec()-1),
targetFrequency_(0.0),
windowSize_(windowSize)
windowSize_(windowSize),
lastTickTime_(0.0),
nodeName_(nodeName)
{
UASSERT(windowSize_ >= 1);
}
@@ -46,7 +48,7 @@ class SyncDiagnostic {
}
diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
diagnosticUpdater_.force_update();
diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this);
diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(5), &SyncDiagnostic::diagnosticTimerCallback, this);
}
void tick(const ros::Time & stamp, double targetFrequency = 0)
@@ -79,16 +81,29 @@ class SyncDiagnostic {
targetFrequency_ = targetFrequency;
}
lastCallbackCalledStamp_ = stamp.toSec();
double clockNow = ros::Time::now().toSec();
if(lastTickTime_ > clockNow)
{
ROS_WARN("%s: Detected time jump in the past of %f sec, forcing diagnostic update.",
nodeName_.c_str(), lastTickTime_ - clockNow);
frequencyStatus_.clear();
diagnosticUpdater_.force_update();
lastCallbackCalledStamp_ = clockNow;
}
else
{
diagnosticUpdater_.update();
}
lastTickTime_ = clockNow;
}
private:
void diagnosticTimerCallback(const ros::TimerEvent& event)
{
diagnosticUpdater_.update();
if(ros::Time::now().toSec()-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
{
ROS_WARN_THROTTLE(5, "%s", topicsNotReceivedWarningMsg_.c_str());
ROS_WARN("%s", topicsNotReceivedWarningMsg_.c_str());
}
}
@@ -103,6 +118,8 @@ private:
double targetFrequency_;
int windowSize_;
std::deque<double> window_;
double lastTickTime_;
std::string nodeName_;
};