Stale inputs detection (#1358)

* 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]>

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -114,6 +114,8 @@ private:
 	double guessMinTranslation_;
 	double guessMinRotation_;
 	double guessMinTime_;
+	double guessLinearVariance_;
+	double guessAngularVariance_;
 	bool publishTf_;
 	bool waitForTransform_;
 	double waitForTransformDuration_;

rtabmap_odom/src/OdometryROS.cpp

@@ -67,6 +67,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	guessMinTranslation_(0.0),
 	guessMinRotation_(0.0),
 	guessMinTime_(0.0),
+	guessLinearVariance_(0.001),
+	guessAngularVariance_(0.001),
 	publishTf_(true),
 	waitForTransform_(true),
 	waitForTransformDuration_(0.1), // 100 ms
@@ -147,6 +149,8 @@ void OdometryROS::onInit()
 	pnh.param("guess_min_translation", guessMinTranslation_, guessMinTranslation_);
 	pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_);
 	pnh.param("guess_min_time", guessMinTime_, guessMinTime_);
+	pnh.param("guess_linear_variance", guessLinearVariance_, guessLinearVariance_);
+	pnh.param("guess_angular_variance", guessAngularVariance_, guessAngularVariance_);
 
 	pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate
 	pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
@@ -186,6 +190,8 @@ void OdometryROS::onInit()
 	NODELET_INFO("Odometry: guess_min_translation  = %f", guessMinTranslation_);
 	NODELET_INFO("Odometry: guess_min_rotation     = %f", guessMinRotation_);
 	NODELET_INFO("Odometry: guess_min_time         = %f", guessMinTime_);
+	NODELET_INFO("Odometry: guess_linear_variance  = %f", guessLinearVariance_);
+	NODELET_INFO("Odometry: guess_angular_variance  = %f", guessAngularVariance_);
 	NODELET_INFO("Odometry: expected_update_rate   = %f Hz", expectedUpdateRate_);
 	NODELET_INFO("Odometry: max_update_rate        = %f Hz", maxUpdateRate_);
 	NODELET_INFO("Odometry: min_update_rate        = %f Hz", minUpdateRate_);
@@ -672,6 +678,44 @@ void OdometryROS::processData()
 		}
 	}
 
+	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_;
+	if(tooOldPreviousData)
+	{
+		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 - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec());
+
+		if(!guess_.isNull())
+		{
+			NODELET_WARN( "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
+					guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
+			odometry_->reset(odometry_->getPose() * guess_);
+			guess_.setNull();
+			guessPreviousPose_.setNull();
+		}
+		else
+		{
+			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
+			Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, this->tfListener(), this->waitForTransformDuration());
+			if(tfPose.isNull())
+			{
+				NODELET_WARN( "Odometry automatically reset to latest computed pose!");
+				odometry_->reset(odometry_->getPose());
+			}
+			else
+			{
+				NODELET_WARN( "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
+						odomFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(tfPose);
+			}
+		}
+	}
+	
+	bool skipOdometryUpdate = false;
+
+	rtabmap::Transform pose;
+	rtabmap::OdometryInfo info;
+	rtabmap::Transform guessVelocity;
 
 	Transform guessCurrentPose;
 	if(!guessFrameId_.empty())
@@ -708,27 +752,22 @@ void OdometryROS::processData()
 				   (guessMinTime_ <= 0.0 || (previousStamp_.toSec()>0.0 && (header.stamp-previousStamp_).toSec() < guessMinTime_)))
 				{
 					// Ignore odometry update, we didn't move enough
-					if(publishTf_)
+					pose = odometry_->getPose() * guess_;
-					{
+					info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1);
-						geometry_msgs::TransformStamped correctionMsg;
+					info.reg.covariance.at<double>(0,0) = guessLinearVariance_;  // xx
-						correctionMsg.child_frame_id = guessFrameId_;
+					info.reg.covariance.at<double>(1,1) = guessLinearVariance_;  // yy
-						correctionMsg.header.frame_id = odomFrameId_;
+					info.reg.covariance.at<double>(2,2) = guessLinearVariance_; // zz
-						correctionMsg.header.stamp = header.stamp;
+					info.reg.covariance.at<double>(3,3) = guessAngularVariance_; // rr
-						Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
+					info.reg.covariance.at<double>(4,4) = guessAngularVariance_; // pp
-						rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
+					info.reg.covariance.at<double>(5,5) = guessAngularVariance_; // yawyaw
-						ros::Time time_now = ros::Time::now();
+
-						if(time_now >= previousClockTime_) {
+					//set velocity
-							tfBroadcaster_.sendTransform(correctionMsg);
+					double dt = (header.stamp-previousStamp_).toSec();
-						}
+					UASSERT(dt>0.0);
-						else {
+					// use part of guess matching dt
-							ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.",
+					(previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-								correctionMsg.header.frame_id.c_str(),
+					guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
-								correctionMsg.child_frame_id.c_str(),
+					skipOdometryUpdate = true;
-								(previousClockTime_ - time_now).toSec());
-						}
-					}
-					guessPreviousPose_ = guessCurrentPose;
-					return;
 				}
 			}
 			guessPreviousPose_ = guessCurrentPose;
@@ -740,23 +779,21 @@ void OdometryROS::processData()
 		}
 	}
 
-	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_;
-
 	// process data
 	ros::WallTime time = ros::WallTime::now();
-	rtabmap::OdometryInfo info;
 	if(!groundTruth.isNull())
 	{
 		data.setGroundTruth(groundTruth);
 	}
-	rtabmap::Transform pose;
+	if(!skipOdometryUpdate)
-	if(!tooOldPreviousData)
 	{
 		pose = odometry_->process(data, guess_, &info);
 	}
 	if(!pose.isNull())
 	{
-		guess_.setNull();
+		if(!skipOdometryUpdate) {
+			guess_.setNull();
+		}
 		resetCurrentCount_ = resetCountdown_;
 
 		//*********************
@@ -829,11 +866,16 @@ void OdometryROS::processData()
 			odom.pose.covariance.at(35) = info.reg.covariance.at<double>(5,5)*2; // yawyaw
 
 			//set velocity
-			bool setTwist = !odometry_->getVelocityGuess().isNull();
+			bool setTwist = !guessVelocity.isNull() || !odometry_->getVelocityGuess().isNull();
 			if(setTwist)
 			{
 				float x,y,z,roll,pitch,yaw;
-				odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				if(skipOdometryUpdate) {
+					UASSERT(!guessVelocity.isNull());
+					guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				} else {
+					odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				}
 				odom.twist.twist.linear.x = x;
 				odom.twist.twist.linear.y = y;
 				odom.twist.twist.linear.z = z;
@@ -878,7 +920,7 @@ void OdometryROS::processData()
 			odomLocalMap_.publish(cloudMsg);
 		}
 
-		if(odomLastFrame_.getNumSubscribers())
+		if(!skipOdometryUpdate && odomLastFrame_.getNumSubscribers())
 		{
 			// check which type of Odometry is using
 			if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry
@@ -1008,20 +1050,14 @@ void OdometryROS::processData()
 		}
 	}
 
-	if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData))
+	if(pose.isNull() && resetCurrentCount_ > 0)
 	{
-		if(tooOldPreviousData)
+		if(--resetCurrentCount_>0)
-		{
-			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 - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec());
-		}
-		else if(--resetCurrentCount_>0)
 		{
 			NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
 		}
 
-		if(resetCurrentCount_ == 0 || tooOldPreviousData)
+		if(resetCurrentCount_ == 0)
 		{
 			if(!guess_.isNull())
 			{
@@ -1184,9 +1220,11 @@ void OdometryROS::processData()
 		msg.header.stamp = header.stamp; // use corresponding time stamp to image
 		odomSensorDataCompressedPub_.publish(msg);
 	}
-
 	double delay = (ros::Time::now() - header.stamp).toSec(); 
-	if(visParams_)
+	if(skipOdometryUpdate) {
+		NODELET_INFO( "Odom: <skipped: guess not moving enough>, std dev=%fm|%frad, update time=%fs, delay=%fs", pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec(), delay);
+	}
+	else if(visParams_)
 	{
 		if(icpParams_)
 		{

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -278,6 +278,7 @@ private:
 	double landmarkDefaultLinVariance_;
 	bool waitForTransform_;
 	double waitForTransformDuration_;
+	double stalenessFactor_;
 	bool useActionForGoal_;
 	bool useSavedMap_;
 	bool genScan_;

rtabmap_slam/src/CoreWrapper.cpp

@@ -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)
@@ -1055,6 +1065,26 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti
 			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;
@@ -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;