merged master->ros2

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

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

rtabmap_odom/src/OdometryROS.cpp

@@ -76,6 +76,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	guessMinTranslation_(0.0),
 	guessMinRotation_(0.0),
 	guessMinTime_(0.0),
+	guessLinearVariance_(0.001),
+	guessAngularVariance_(0.001),
 	publishTf_(true),
 	waitForTransform_(0.1), // 100 ms
 	publishNullWhenLost_(true),
@@ -143,6 +145,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	guessMinTranslation_ = this->declare_parameter("guess_min_translation", guessMinTranslation_);
 	guessMinRotation_ = this->declare_parameter("guess_min_rotation", guessMinRotation_);
 	guessMinTime_ = this->declare_parameter("guess_min_time", guessMinTime_);
+	guessLinearVariance_ = this->declare_parameter("guess_linear_variance", guessLinearVariance_);
+	guessAngularVariance_ = this->declare_parameter("guess_angular_variance", guessAngularVariance_);
 
 	expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_);
 	maxUpdateRate_ = this->declare_parameter("max_update_rate", maxUpdateRate_);
@@ -205,6 +209,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_translation  = %f", guessMinTranslation_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_rotation     = %f", guessMinRotation_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_time         = %f", guessMinTime_);
+	RCLCPP_INFO(this->get_logger(), "Odometry: guess_linear_variance  = %f", guessLinearVariance_);
+	RCLCPP_INFO(this->get_logger(), "Odometry: guess_angular_variance = %f", guessAngularVariance_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: expected_update_rate   = %f Hz", expectedUpdateRate_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate        = %f Hz", maxUpdateRate_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate        = %f Hz", minUpdateRate_);
@@ -710,6 +716,44 @@ void OdometryROS::processData()
 		}
 	}
 
+	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
+	if(tooOldPreviousData)
+	{
+		RCLCPP_WARN(this->get_logger(), "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.",
+				rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
+
+		if(!guess_.isNull())
+		{
+			RCLCPP_WARN(this->get_logger(), "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, *tfBuffer_, waitForTransform_);
+			if(tfPose.isNull())
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
+				odometry_->reset(odometry_->getPose());
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "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())
@@ -746,28 +790,22 @@ void OdometryROS::processData()
 				   (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ < 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::msg::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
 
-						double time_now = now().seconds();
+					//set velocity
-						if(time_now >= previousClockTime_) {
+					double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_;
-							tfBroadcaster_->sendTransform(correctionMsg);
+					UASSERT(dt>0.0);
-						}
+					// use part of guess matching dt
-						else {
+					(previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-							RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.",
+					guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
-								correctionMsg.header.frame_id.c_str(),
+					skipOdometryUpdate = true;
-								correctionMsg.child_frame_id.c_str(),
-								previousClockTime_ - time_now);
-						}
-					}
-					guessPreviousPose_ = guessCurrentPose;
-					return;
 				}
 			}
 			guessPreviousPose_ = guessCurrentPose;
@@ -779,23 +817,21 @@ void OdometryROS::processData()
 		}
 	}
 
-	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_;
-
 	// process data
 	rclcpp::Time timeStart = rclcpp::Clock().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_;
 
 		//*********************
@@ -869,11 +905,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;
@@ -918,7 +959,7 @@ void OdometryROS::processData()
 			odomLocalMap_->publish(cloudMsg);
 		}
 
-		if(odomLastFrame_->get_subscription_count())
+		if(!skipOdometryUpdate && odomLastFrame_->get_subscription_count())
 		{
 			// check which type of Odometry is using
 			if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry
@@ -1049,20 +1090,14 @@ void OdometryROS::processData()
 
 	}
 
-	if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData))
+	if(pose.isNull() && resetCurrentCount_ > 0)
 	{
-		if(tooOldPreviousData)
+		if(--resetCurrentCount_>0)
-		{
-			RCLCPP_WARN(this->get_logger(), "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.",
-					rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
-		}
-		else if(--resetCurrentCount_>0)
 		{
 			RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
 		}
 
-		if(resetCurrentCount_ == 0 || tooOldPreviousData)
+		if(resetCurrentCount_ == 0)
 		{
 			if(!guess_.isNull())
 			{
@@ -1225,9 +1260,11 @@ void OdometryROS::processData()
 		msg.header.stamp = header.stamp; // use corresponding time stamp to image
 		odomSensorDataCompressedPub_->publish(msg);
 	}
-
 	double delay =  (now()-header.stamp).seconds(); 
-	if(visParams_)
+	if(skipOdometryUpdate) {
+		RCLCPP_INFO(this->get_logger(), "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)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
+	}
+	else if(visParams_)
 	{
 		if(icpParams_)
 		{

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

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

rtabmap_slam/src/CoreWrapper.cpp

@@ -121,6 +121,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		landmarkDefaultAngVariance_(0.001),
 		landmarkDefaultLinVariance_(0.001),
 		waitForTransform_(0.2),// 200 ms
+		stalenessFactor_(0.0),
 		useActionForGoal_(false),
 		useSavedMap_(true),
 		genScan_(false),
@@ -207,6 +208,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	
 	pubLocPoseOnlyWhenLocalizing_ = this->declare_parameter("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_);
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
+	stalenessFactor_ = this->declare_parameter("staleness_factor", stalenessFactor_);
 	initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr);
 	useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_);
 #ifndef WITH_NAV2_MSGS
@@ -251,6 +253,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	RCLCPP_INFO(this->get_logger(), "rtabmap: odom_sensor_sync   = %s", odomSensorSync_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "rtabmap: wait_for_transform = %f", waitForTransform_);
+	if(stalenessFactor_!=0.0 && stalenessFactor_ < 1.0) {
+		RCLCPP_ERROR(this->get_logger(), "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;
+	}
+	RCLCPP_INFO(this->get_logger(), "rtabmap: staleness_factor = %f", stalenessFactor_);
 	if(this->isSubscribedToStereo())
 	{
 		RCLCPP_INFO(this->get_logger(), "rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
@@ -1138,6 +1146,26 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti
 			UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg.pose.covariance[0], odomMsg.twist.covariance[0]));
 			triggerNewMapBeforeNextUpdate_ = true;
 			lastPoseCovariance_ = cv::Mat();
+		} 
+		else if(stalenessFactor_>0.0 && 
+			    previousStamp_.seconds() > 0.0 && 
+				rate_>0.0f && 
+				(stamp - previousStamp_).seconds() > 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 - previousStamp_).seconds(),
+				stamp.seconds(),
+				previousStamp_.seconds(),
+				Parameters::kRtabmapDetectionRate().c_str(),
+				rate_,
+				1.0f/rate_,
+				stalenessFactor_,
+				stalenessFactor_/rate_);
+			triggerNewMapBeforeNextUpdate_ = true;
+			lastPoseCovariance_ = cv::Mat();
 		}
 
 		lastPoseIntermediate_ = false;
@@ -1229,6 +1257,26 @@ bool CoreWrapper::odomTFUpdate(const std::string & odomFrameId, const rclcpp::Ti
 			triggerNewMapBeforeNextUpdate_ = true;
 			lastPoseCovariance_ = cv::Mat();
 		}
+		else if(stalenessFactor_>0.0 && 
+			    previousStamp_.seconds() > 0.0 && 
+				rate_>0.0f && 
+				(stamp - previousStamp_).seconds() > 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 - previousStamp_).seconds(),
+				stamp.seconds(),
+				previousStamp_.seconds(),
+				Parameters::kRtabmapDetectionRate().c_str(),
+				rate_,
+				1.0f/rate_,
+				stalenessFactor_,
+				stalenessFactor_/rate_);
+			triggerNewMapBeforeNextUpdate_ = true;
+			lastPoseCovariance_ = cv::Mat();
+		}
 
 		lastPoseIntermediate_ = false;
 		lastPose_ = odom;

rtabmap_util/src/MapsManager.cpp

@@ -526,7 +526,9 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 			if(!iter->second.isNull())
 			{
 				rtabmap::SensorData data;
-				if(iter->first == 0 || (addedNodes->find(iter->first) == addedNodes->end() && !uContains(localMaps_.localGrids(), iter->first)))
+				if(iter->first == 0 || (
+					(fullUpdateNeeded || addedNodes->find(iter->first) == addedNodes->end()) && 
+						!uContains(localMaps_.localGrids(), iter->first)))
 				{
 					UDEBUG("Data required for %d", iter->first);
 					std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);