GPS and global pose async topics buffered to get more accuratly the closest value to current update stamp in case there is significant delay.

rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h

@@ -323,6 +323,42 @@ inline int sizeOfPointField(int datatype)
   }
   return -1;
 }
+
+template <typename K, typename V>
+typename std::map<K, V>::const_iterator getClosestIterator(
+	const std::map<K, V> & buffer,
+	const K & key)
+{
+	UASSERT(!buffer.empty());
+	typename std::map<K, V>::const_iterator iterB = buffer.lower_bound(key);
+	typename std::map<K, V>::const_iterator iterA = iterB;
+	if(iterA != buffer.begin())
+	{
+		iterA = --iterA;
+	}
+	if(iterB == buffer.end())
+	{
+		iterB = --iterB;
+	}
+	if(iterA == iterB)
+	{
+		return iterA;
+	}
+	if(iterA->first > key)
+	{
+		return iterA;
+	}
+	else if(iterB->first < key)
+	{
+		return iterB;
+	}
+	else if(key - iterA->first < iterB->first - key)
+	{
+		return iterA;
+	}
+	return iterB;
+}
+
 }
 
 #endif /* MSGCONVERSION_H_ */

rtabmap_conversions/src/MsgConversion.cpp

@@ -3386,7 +3386,7 @@ bool deskew_impl(
 			}
 		}
 	}
-	UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
+	UDEBUG("Lidar deskewing time=%fs (slerp=%s waitForTransform=%f)", processingTime.elapsed(), slerp?"true":"false", waitForTransform);
 	return true;
 }
 

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -405,10 +405,15 @@ private:
 	cv::Mat userData_;
 	UMutex userDataMutex_;
 
+	rclcpp::CallbackGroup::SharedPtr globalPoseAsyncCallbackGroup_;
 	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr globalPoseAsyncSub_;
-	geometry_msgs::msg::PoseWithCovarianceStamped globalPose_;
+	std::map<double, geometry_msgs::msg::PoseWithCovarianceStamped> globalPoses_;
+	UMutex globalPoseMutex_;
+
+	rclcpp::CallbackGroup::SharedPtr gpsAsyncCallbackGroup_;
 	rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixAsyncSub_;
-	rtabmap::GPS gps_;
+	std::map<double, rtabmap::GPS> gps_;
+	UMutex gpsMutex_;
 
 	rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_;
 	rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmarkDetectionSub_;

rtabmap_slam/src/CoreWrapper.cpp

@@ -145,7 +145,6 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	char * rosHomePath = getenv("ROS_HOME");
 	std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
 	databasePath_ = workingDir+"/"+rtabmap::Parameters::getDefaultDatabaseName();
-	globalPose_.header.stamp = rclcpp::Time(0);
 
 	mapsManager_.init(*this, this->get_name(), true);
 
@@ -844,12 +843,18 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 
 	// Setup callback groups for any subscriptions that should not be affected by main processing thread.
 	userDataAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+	globalPoseAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+	gpsAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 	landmarkCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 	imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 	rclcpp::SubscriptionOptions userDataAsyncSubOptions;
+	rclcpp::SubscriptionOptions globalPoseAsyncSubOptions;
+	rclcpp::SubscriptionOptions gpsAsyncSubOptions;
 	rclcpp::SubscriptionOptions landmarkSubOptions;
 	rclcpp::SubscriptionOptions imuSubOptions;
 	userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_;
+	globalPoseAsyncSubOptions.callback_group = globalPoseAsyncCallbackGroup_;
+	gpsAsyncSubOptions.callback_group = gpsAsyncCallbackGroup_;
 	landmarkSubOptions.callback_group = imuCallbackGroup_;
 	imuSubOptions.callback_group = imuCallbackGroup_;
 
@@ -858,8 +863,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	qosGPS = this->declare_parameter("qos_gps", qosGPS);
 	qosIMU = this->declare_parameter("qos_imu", qosIMU);
 	userDataAsyncSub_ = this->create_subscription<rtabmap_msgs::msg::UserData>("user_data_async", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1), userDataAsyncSubOptions);
-	globalPoseAsyncSub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), subOptions);
+	globalPoseAsyncSub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), globalPoseAsyncSubOptions);
-	gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), subOptions);
+	gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), gpsAsyncSubOptions);
 	landmarkDetectionSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
 	landmarkDetectionsSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetections>("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
 #ifdef WITH_APRILTAG_MSGS
@@ -2059,18 +2064,40 @@ void CoreWrapper::process(
 		data.setGroundTruth(groundTruthPose);
 
 		//global pose
-		if(globalPose_.header.stamp.sec != 0 || globalPose_.header.stamp.nanosec != 0)
+		geometry_msgs::msg::PoseWithCovarianceStamped globalPoseMsg;
+		globalPoseMsg.header.stamp = rclcpp::Time(0);
+		{
+			UScopeMutex lock(globalPoseMutex_);
+			if(!globalPoses_.empty())
+			{
+				auto iter = rtabmap_conversions::getClosestIterator<double, geometry_msgs::msg::PoseWithCovarianceStamped>(globalPoses_, data.stamp());
+				// Check if it is not too old
+				if(rate_ == 0 || fabs(iter->first - data.stamp()) < 1.0/rate_)
+				{
+					globalPoseMsg = iter->second;
+				}
+				else
+				{
+					RCLCPP_WARN(this->get_logger(), "Ignoring global pose with stamp %f because it should be inside the update period (%f) of the current data stamp (%f).",
+						iter->first,
+						1.0/rate_,
+						data.stamp());
+				}
+				globalPoses_.clear();
+			}
+		}
+		if(globalPoseMsg.header.stamp.sec != 0 || globalPoseMsg.header.stamp.nanosec != 0)
 		{
 			// assume sensor is fixed
 			Transform sensorToBase = rtabmap_conversions::getTransform(
-					globalPose_.header.frame_id,
+					globalPoseMsg.header.frame_id,
 					frameId_,
 					stamp,
 					*tfBuffer_,
 					waitForTransform_);
 			if(!sensorToBase.isNull())
 			{
-				Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPose_.pose.pose);
+				Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPoseMsg.pose.pose);
 				globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
 
 				// Correction of the global pose accounting the odometry movement since we received it
@@ -2078,7 +2105,7 @@ void CoreWrapper::process(
 						frameId_,
 						odomFrameId,
 						stamp,
-						rclcpp::Time(globalPose_.header.stamp.sec, globalPose_.header.stamp.nanosec),
+						rclcpp::Time(globalPoseMsg.header.stamp.sec, globalPoseMsg.header.stamp.nanosec),
 						*tfBuffer_,
 						waitForTransform_);
 				if(!correction.isNull())
@@ -2091,17 +2118,32 @@ void CoreWrapper::process(
 							"If odometry is small since it received the global pose and "
 							"covariance is large, this should not be a problem.");
 				}
-				cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
+				cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPoseMsg.pose.covariance.data()).clone();
 				data.setGlobalPose(globalPose, globalPoseCovariance);
 			}
 		}
-		globalPose_.header.stamp = rclcpp::Time(0);
 
-		if(gps_.stamp() > 0.0)
 		{
-			data.setGPS(gps_);
+			UScopeMutex lock(gpsMutex_);
+			if(!gps_.empty())
+			{
+				std::map<double, rtabmap::GPS>::const_iterator iter = rtabmap_conversions::getClosestIterator<double, rtabmap::GPS>(gps_, data.stamp());
+				// Check if it is not too old
+				if(rate_ == 0 || fabs(iter->first - data.stamp()) < 1.0/rate_)
+				{
+					data.setGPS(iter->second);
+				}
+				else
+				{
+					RCLCPP_WARN(this->get_logger(), "Ignoring GPS with stamp %f because it should be inside the update period (%f) of the current data stamp (%f).",
+						iter->first,
+						1.0/rate_,
+						data.stamp());
+				}
+				gps_.clear();
+			}
 		}
-		gps_ = rtabmap::GPS();
+		
 
 		//tag detections
 		landmarksMutex_.lock();
@@ -2520,7 +2562,12 @@ void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::msg::PoseWithCova
 {
 	if(!paused_)
 	{
-		globalPose_ = *globalPoseMsg;
+		UScopeMutex lock(globalPoseMutex_);
+		globalPoses_.insert(std::make_pair(rtabmap_conversions::timestampFromROS(globalPoseMsg->header.stamp), *globalPoseMsg));
+		if(globalPoses_.size() > 1000)
+		{
+			globalPoses_.erase(globalPoses_.begin());
+		}
 	}
 }
 
@@ -2537,13 +2584,21 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedP
 				error = sqrt(variance);
 			}
 		}
-		gps_ = rtabmap::GPS(
+
-				rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp),
+		rtabmap::GPS gps(
-				gpsFixMsg->longitude,
+			rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp),
-				gpsFixMsg->latitude,
+			gpsFixMsg->longitude,
-				gpsFixMsg->altitude,
+			gpsFixMsg->latitude,
-				error,
+			gpsFixMsg->altitude,
-				0);
+			error,
+			0);
+
+		UScopeMutex lock(gpsMutex_);
+		gps_.insert(std::make_pair(gps.stamp(), gps));
+		if(gps_.size() > 1000)
+		{
+			gps_.erase(gps_.begin());
+		}
 	}
 }
 
@@ -3003,8 +3058,8 @@ void CoreWrapper::resetRtabmapCallback(
 	graphLatched_ = false;
 	mapsManager_.clear();
 	previousStamp_ = rclcpp::Time(0);
-	globalPose_.header.stamp = rclcpp::Time(0);
+	globalPoses_.clear();
-	gps_ = rtabmap::GPS();
+	gps_.clear();
 	landmarksMutex_.lock();
 	landmarks_.clear();
 	landmarksMutex_.unlock();
@@ -3105,8 +3160,8 @@ void CoreWrapper::loadDatabaseCallback(
 	graphLatched_ = false;
 	mapsManager_.clear();
 	previousStamp_ = rclcpp::Time(0);
-	globalPose_.header.stamp = rclcpp::Time(0);
+	globalPoses_.clear();
-	gps_ = rtabmap::GPS();
+	gps_.clear();
 	landmarksMutex_.lock();
 	landmarks_.clear();
 	landmarksMutex_.unlock();
@@ -3241,8 +3296,8 @@ void CoreWrapper::backupDatabaseCallback(
 	userDataMutex_.lock();
 	userData_ = cv::Mat();
 	userDataMutex_.unlock();
-	globalPose_.header.stamp = rclcpp::Time(0);
+	globalPoses_.clear();
-	gps_ = rtabmap::GPS();
+	gps_.clear();
 	landmarksMutex_.lock();
 	landmarks_.clear();
 	landmarksMutex_.unlock();