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(*msg); ++navmap_msg_count_;