From 25ce1feca57c94f519573941149396ab60565195 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= <fmrico@gmail.com>
Date: Tue, 23 Sep 2025 10:31:40 +0200
Subject: [PATCH] Fix tranformations sensible to frame_id
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
---
 navmap_rviz_plugin/src/NavMapDisplay.cpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/navmap_rviz_plugin/src/NavMapDisplay.cpp b/navmap_rviz_plugin/src/NavMapDisplay.cpp
index 825dddb..f377532 100644
--- a/navmap_rviz_plugin/src/NavMapDisplay.cpp
+++ b/navmap_rviz_plugin/src/NavMapDisplay.cpp
@@ -207,6 +207,21 @@ void NavMapDisplay::onDisable()
 
 void NavMapDisplay::processMessage(const NavMapMsg::ConstSharedPtr msg)
 {
+  Ogre::Vector3 position;
+  Ogre::Quaternion orientation;
+  geometry_msgs::msg::Pose identity;
+
+  if (!context_->getFrameManager()->transform(
+        msg->header, identity, position, orientation))
+  {
+    setStatus(rviz_common::properties::StatusProperty::Error,
+              "TF", "Unable to transform " + QString::fromStdString(msg->header.frame_id));
+    return;
+  }
+
+  root_node_->setPosition(position);
+  root_node_->setOrientation(orientation);
+
   last_msg_ = std::make_shared<NavMapMsg>(*msg);
 
   ++navmap_msg_count_;