From c4a596f8008428717ce5c45e48a5798e4e079235 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 1 Mar 2025 15:50:39 -0800
Subject: [PATCH] Support any frame_id in initialpose #1273

---
 rtabmap_slam/src/CoreWrapper.cpp | 37 +++++++++++++++++++++++++++-----
 1 file changed, 32 insertions(+), 5 deletions(-)

diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp
index 08b56b4b..a26a58e3 100644
--- a/rtabmap_slam/src/CoreWrapper.cpp
+++ b/rtabmap_slam/src/CoreWrapper.cpp
@@ -2526,14 +2526,41 @@ void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1,
 
 void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
 {
-	Transform intialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
-	if(intialPose.isNull())
+	Transform mapToPose = Transform::getIdentity();
+	if(msg->header.frame_id.empty())
 	{
-		NODELET_ERROR("Pose received is null!");
-		return;
+		NODELET_WARN("Received initialpose doesn't have frame_id set, assuming it is in %s frame.", mapFrameId_.c_str());
+	}
+	else if(msg->header.frame_id != mapFrameId_)
+	{
+		mapToPose = rtabmap_conversions::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
+		if(mapToPose.isNull())
+		{
+			NODELET_ERROR("Failed to transform initialpose from frame %s to map frame %s", msg->header.frame_id.c_str(), mapFrameId_.c_str());
+			return;
+		}
 	}
 
-	rtabmap_.setInitialPose(intialPose);
+	Transform initialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
+	if(initialPose.isNull())
+	{
+		NODELET_ERROR("initialpose received is null!");
+		return;
+	}
+	if(mapToPose.isIdentity())
+	{
+		NODELET_INFO("initialpose received: %s", initialPose.prettyPrint().c_str());
+		rtabmap_.setInitialPose(initialPose);
+	}
+	else
+	{
+		NODELET_INFO("initialpose received: %s in %s frame, transformed to %s in %s frame.",
+			initialPose.prettyPrint().c_str(),
+			msg->header.frame_id.c_str(),
+			(mapToPose * initialPose).prettyPrint().c_str(),
+			mapFrameId_.c_str());
+		rtabmap_.setInitialPose(mapToPose*initialPose);
+	}
 }
 
 void CoreWrapper::goalCommonCallback(