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

2026-10-07 02:07:45 +08:00 · 2025-03-30 14:15:09 -07:00
parent a2f2971094
commit e661abb884
4 changed files with 125 additions and 29 deletions
@@ -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);
-	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);
+	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), 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),
-				gpsFixMsg->longitude,
-				gpsFixMsg->latitude,
-				gpsFixMsg->altitude,
-				error,
-				0);
+
+		rtabmap::GPS gps(
+			rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp),
+			gpsFixMsg->longitude,
+			gpsFixMsg->latitude,
+			gpsFixMsg->altitude,
+			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);
-	gps_ = rtabmap::GPS();
+	globalPoses_.clear();
+	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);
-	gps_ = rtabmap::GPS();
+	globalPoses_.clear();
+	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);
-	gps_ = rtabmap::GPS();
+	globalPoses_.clear();
+	gps_.clear();
 	landmarksMutex_.lock();
 	landmarks_.clear();
 	landmarksMutex_.unlock();