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

* 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

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -87,7 +87,7 @@ protected:
 	tf::TransformListener & tfListener() {return tfListener_;}
 	double waitForTransformDuration() const {return waitForTransform_?waitForTransformDuration_:0.0;}
 	rtabmap::Transform velocityGuess() const;
-	double previousStamp() const {return previousStamp_;}
+	ros::Time previousStamp() const {return previousStamp_;}
 	virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {}
 
 private:
@@ -158,7 +158,8 @@ private:
 	bool icpParams_;
 	rtabmap::Transform guess_;
 	rtabmap::Transform guessPreviousPose_;
-	double previousStamp_;
+	ros::Time previousStamp_;
+	ros::Time previousClockTime_;
 	double expectedUpdateRate_;
 	double maxUpdateRate_;
 	double minUpdateRate_;

rtabmap_odom/src/OdometryROS.cpp

@@ -78,7 +78,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	stereoParams_(stereoParams),
 	visParams_(visParams),
 	icpParams_(icpParams),
-	previousStamp_(0.0),
 	expectedUpdateRate_(0.0),
 	maxUpdateRate_(0.0),
 	minUpdateRate_(0.0),
@@ -543,27 +542,56 @@ void OdometryROS::mainLoop()
 	Transform groundTruth;
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
-		if(previousStamp_>0.0 && previousStamp_ >= header.stamp.toSec())
+		// Detect time jump in the past
+		ros::Time clockNow = ros::Time::now();
+		if(previousClockTime_ > clockNow)
+		{
+			NODELET_WARN("Odometry: Detected jump back in time of %f sec. Odometry is "
+				"automatically reset to latest computed pose!",
+				(previousClockTime_ - clockNow).toSec());
+			SensorData dataCpy = dataToProcess_;
+			std_msgs::Header headerCpy = dataHeaderToProcess_;
+			ros::Time previousCpy = previousClockTime_;
+			this->reset(odometry_->getPose());
+			if(previousCpy > headerCpy.stamp) {
+				// new frame is using new clock, process it now
+				dataToProcess_ = dataCpy;
+				dataHeaderToProcess_ = headerCpy;
+				dataReady_.release();
+				NODELET_WARN("Odometry: Restarting with frame: %f (clock previous=%f, new=%f)",
+					headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec());
+			}
+			else {
+				// skip that old frame
+				NODELET_WARN("Odometry: skipping frame: %f (clock previous=%f, new=%f)",
+					headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec());
+			}
+			previousClockTime_ = clockNow;
+			return;
+		}
+		previousClockTime_ = clockNow;
+
+		if(previousStamp_ >= header.stamp)
 		{
 			NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). "
-					"New stamp should be always greater than previous stamp. This new data is ignored.",
-					previousStamp_, header.stamp.toSec());
+					"New stamp should be always greater than previous stamp. This new data is ignored. ",
+					previousStamp_.toSec(), header.stamp.toSec());
 			return;
 		}
 		else if(maxUpdateRate_ > 0 &&
-				previousStamp_ > 0 &&
-				(header.stamp.toSec()-previousStamp_+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_)
+				previousStamp_.toSec() > 0 &&
+				((header.stamp-previousStamp_).toSec()+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_)
 		{
 			// throttling
 			return;
 		}
 		else if(maxUpdateRate_ == 0 &&
 				expectedUpdateRate_ > 0 &&
-			    previousStamp_ > 0 &&
-			    (header.stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_)
+			    previousStamp_.toSec() > 0 &&
+			    (header.stamp-previousStamp_).toSec() < 1.0/expectedUpdateRate_)
 		{
 			NODELET_WARN("Odometry: Aborting odometry update, higher frame rate detected (%f Hz) than the expected one (%f Hz). (stamps: previous=%fs new=%fs)",
-					1.0/(header.stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, header.stamp.toSec());
+					1.0/(header.stamp-previousStamp_).toSec(), expectedUpdateRate_, previousStamp_.toSec(), header.stamp.toSec());
 			return;
 		}
 
@@ -632,7 +660,7 @@ void OdometryROS::mainLoop()
 				guess_.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 				if((guessMinTranslation_ <= 0.0 || uMax3(fabs(x), fabs(y), fabs(z)) < guessMinTranslation_) &&
 				   (guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_) &&
-				   (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && header.stamp.toSec()-previousStamp_ < guessMinTime_)))
+				   (guessMinTime_ <= 0.0 || (previousStamp_.toSec()>0.0 && (header.stamp-previousStamp_).toSec() < guessMinTime_)))
 				{
 					// Ignore odometry update, we didn't move enough
 					if(publishTf_)
@@ -643,7 +671,16 @@ void OdometryROS::mainLoop()
 						correctionMsg.header.stamp = header.stamp;
 						Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
 						rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
-						tfBroadcaster_.sendTransform(correctionMsg);
+						ros::Time time_now = ros::Time::now();
+						if(time_now >= previousClockTime_) {
+							tfBroadcaster_.sendTransform(correctionMsg);
+						}
+						else {
+							ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.",
+								correctionMsg.header.frame_id.c_str(),
+								correctionMsg.child_frame_id.c_str(),
+								(previousClockTime_ - time_now).toSec());
+						}
 					}
 					guessPreviousPose_ = guessCurrentPose;
 					return;
@@ -658,7 +695,7 @@ void OdometryROS::mainLoop()
 		}
 	}
 
-	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (header.stamp.toSec()-previousStamp_) > 1.0/minUpdateRate_;
+	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_;
 
 	// process data
 	ros::WallTime time = ros::WallTime::now();
@@ -697,11 +734,29 @@ void OdometryROS::mainLoop()
 				correctionMsg.header.stamp = header.stamp;
 				Transform correction = pose * guessCurrentPose.inverse();
 				rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
-				tfBroadcaster_.sendTransform(correctionMsg);
+				ros::Time time_now = ros::Time::now();
+				if(time_now >= previousClockTime_) {
+					tfBroadcaster_.sendTransform(correctionMsg);
+				}
+				else {
+					ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.",
+						correctionMsg.header.frame_id.c_str(),
+						correctionMsg.child_frame_id.c_str(),
+						(previousClockTime_ - time_now).toSec());
+				}
 			}
 			else
 			{
-				tfBroadcaster_.sendTransform(poseMsg);
+				ros::Time time_now = ros::Time::now();
+				if(time_now >= previousClockTime_) {
+					tfBroadcaster_.sendTransform(poseMsg);
+				}
+				else {
+					ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.",
+						poseMsg.header.frame_id.c_str(),
+						poseMsg.child_frame_id.c_str(),
+						(previousClockTime_ - time_now).toSec());
+				}
 			}
 		}
 
@@ -893,7 +948,18 @@ void OdometryROS::mainLoop()
 			correctionMsg.header.stamp = header.stamp;
 			Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
 			rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
-			tfBroadcaster_.sendTransform(correctionMsg);
+			ros::Time time_now = ros::Time::now();
+			if(time_now >= previousClockTime_) {
+				tfBroadcaster_.sendTransform(correctionMsg);
+			}
+			else {
+				ROS_WARN("TF %s->%s is not published because its stamp (%f) is greater "
+					"than current time (%f), possible time jump happened!",
+					correctionMsg.header.frame_id.c_str(),
+					correctionMsg.child_frame_id.c_str(),
+					correctionMsg.header.stamp.toSec(),
+					time_now.toSec());
+			}
 		}
 	}
 
@@ -903,7 +969,7 @@ void OdometryROS::mainLoop()
 		{
 			NODELET_WARN( "Odometry lost! Odometry will be reset because last update "
 					"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
-					header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec());
+					(header.stamp - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec());
 		}
 		else if(--resetCurrentCount_>0)
 		{
@@ -1098,10 +1164,10 @@ void OdometryROS::mainLoop()
 		syncDiagnostic_->tick(header.stamp,
 			maxUpdateRate_>0 ? maxUpdateRate_:
 			expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
-			previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate);
+			previousStamp_.toSec() == 0.0 || (header.stamp - previousStamp_).toSec() > 1.0/curentRate?0:curentRate);
 	}
 
-	previousStamp_ = header.stamp.toSec();
+	previousStamp_ = header.stamp;
 }
 
 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -1125,7 +1191,8 @@ void OdometryROS::reset(const Transform & pose)
 	odometry_->reset(pose);
 	guess_.setNull();
 	guessPreviousPose_.setNull();
-	previousStamp_ = 0.0;
+	previousStamp_ = ros::Time();
+	previousClockTime_ = ros::Time();
 	resetCurrentCount_ = resetCountdown_;
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();
@@ -1135,6 +1202,7 @@ void OdometryROS::reset(const Transform & pose)
 	imus_.clear();
 	imuMutex_.unlock();
 	this->flushCallbacks();
+	this->tfListener().clear();
 }
 
 bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -380,11 +380,11 @@ private:
 					-1.0,
 					laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
 
-			if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
+			if(guessFrameId().empty() && previousStamp().toSec() > 0.0 && !velocityGuess().isNull())
 			{
 				// deskew with constant velocity model (we are in frameId)
 				sensor_msgs::PointCloud2 scanOutDeskewed;
-				if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+				if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess()))
 				{
 					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
 					return;
@@ -405,11 +405,11 @@ private:
 		{
 			projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
 
-			if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull())
+			if(deskewing_ && previousStamp().toSec() > 0.0 && !velocityGuess().isNull())
 			{
 				// deskew with constant velocity model
 				sensor_msgs::PointCloud2 scanOutDeskewed;
-				if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+				if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess()))
 				{
 					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
 					return;
@@ -628,7 +628,7 @@ private:
 					return;
 				}
 			}
-			else if(previousStamp() > 0 && !velocityGuess().isNull())
+			else if(previousStamp().toSec() > 0.0 && !velocityGuess().isNull())
 			{
 				// deskew with constant velocity model
 				bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0;
@@ -648,7 +648,7 @@ private:
 				}
 
 				sensor_msgs::PointCloud2::Ptr cloudDeskewed(new sensor_msgs::PointCloud2);
-				if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
+				if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp().toSec(), velocityGuess()))
 				{
 					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
 					return;

rtabmap_slam/src/CoreWrapper.cpp

@@ -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())

rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h

@@ -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_;
 
 };