Fix fixed frame issues of the RViz SubmapsDisplay. (#454)

Adds a 'map_node_' which gets updated with the transform to RViz's
fixed frame. Before, submaps rendered incorrectly if a fixed frame
different from the map frame was chosen.

Removes the map frame property since this data is published on the
submap list topic.

Renames node names to reflect the frames they represent.

cartographer_ros/configuration_files/demo_2d.rviz

@@ -133,7 +133,6 @@ Visualization Manager:
       Visual Enabled: true
     - Class: Submaps
       Enabled: true
-      Map frame: map
       Name: Submaps
       Submap query service: /submap_query
       Topic: /submap_list

cartographer_ros/configuration_files/demo_3d.rviz

@@ -195,7 +195,6 @@ Visualization Manager:
       Value: false
     - Class: Submaps
       Enabled: true
-      Map frame: map
       Name: Submaps
       Submap query service: /submap_query
       Topic: /submap_list

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -46,16 +46,15 @@ constexpr float kAlphaUpdateThreshold = 0.2f;
 
 DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
                                ::rviz::DisplayContext* const display_context,
+                               Ogre::SceneNode* const map_node,
                                ::rviz::Property* const submap_category,
                                const bool visible, const float pose_axes_length,
                                const float pose_axes_radius)
     : id_(id),
       display_context_(display_context),
-      scene_node_(display_context->getSceneManager()
+      submap_node_(map_node->createChildSceneNode()),
-                      ->getRootSceneNode()
+      ogre_submap_(id, display_context->getSceneManager(), submap_node_),
-                      ->createChildSceneNode()),
+      pose_axes_(display_context->getSceneManager(), submap_node_,
-      ogre_submap_(id, display_context->getSceneManager(), scene_node_),
-      pose_axes_(display_context->getSceneManager(), scene_node_,
                  pose_axes_length, pose_axes_radius),
       last_query_timestamp_(0) {
   // DrawableSubmap creates and manages its visibility property object
@@ -65,7 +64,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
   visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>(
       "" /* title */, visible, "" /* description */, submap_category,
       SLOT(ToggleVisibility()), this);
-  scene_node_->setVisible(visible);
+  submap_node_->setVisible(visible);
   connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
 }
 
@@ -75,24 +74,17 @@ DrawableSubmap::~DrawableSubmap() {
   if (QueryInProgress()) {
     rpc_request_future_.wait();
   }
-  display_context_->getSceneManager()->destroySceneNode(scene_node_);
+  display_context_->getSceneManager()->destroySceneNode(submap_node_);
 }
 
 void DrawableSubmap::Update(
     const ::std_msgs::Header& header,
-    const ::cartographer_ros_msgs::SubmapEntry& metadata,
+    const ::cartographer_ros_msgs::SubmapEntry& metadata) {
-    ::rviz::FrameManager* const frame_manager) {
-  Ogre::Vector3 position;
-  Ogre::Quaternion orientation;
-  if (!frame_manager->transform(header, metadata.pose, position, orientation)) {
-    // We don't know where we would display the texture, so we stop here.
-    return;
-  }
   ::cartographer::common::MutexLocker locker(&mutex_);
   metadata_version_ = metadata.submap_version;
   pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
-  scene_node_->setPosition(ToOgre(pose_.translation()));
+  submap_node_->setPosition(ToOgre(pose_.translation()));
-  scene_node_->setOrientation(ToOgre(pose_.rotation()));
+  submap_node_->setOrientation(ToOgre(pose_.rotation()));
   display_context_->queueRender();
   visibility_->setName(
       QString("%1.%2").arg(id_.submap_index).arg(metadata_version_));
@@ -164,7 +156,7 @@ void DrawableSubmap::UpdateSceneNode() {
 }
 
 void DrawableSubmap::ToggleVisibility() {
-  scene_node_->setVisible(visibility_->getBool());
+  submap_node_->setVisible(visibility_->getBool());
   display_context_->queueRender();
 }
 

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -47,8 +47,8 @@ class DrawableSubmap : public QObject {
  public:
   DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id,
                  ::rviz::DisplayContext* display_context,
-                 ::rviz::Property* submap_category, bool visible,
+                 Ogre::SceneNode* map_node, ::rviz::Property* submap_category,
-                 float pose_axes_length, float pose_axes_radius);
+                 bool visible, float pose_axes_length, float pose_axes_radius);
   ~DrawableSubmap() override;
   DrawableSubmap(const DrawableSubmap&) = delete;
   DrawableSubmap& operator=(const DrawableSubmap&) = delete;
@@ -56,8 +56,7 @@ class DrawableSubmap : public QObject {
   // Updates the 'metadata' for this submap. If necessary, the next call to
   // MaybeFetchTexture() will fetch a new submap texture.
   void Update(const ::std_msgs::Header& header,
-              const ::cartographer_ros_msgs::SubmapEntry& metadata,
+              const ::cartographer_ros_msgs::SubmapEntry& metadata);
-              ::rviz::FrameManager* frame_manager);
 
   // If an update is needed, it will send an RPC using 'client' to request the
   // new data for the submap and returns true.
@@ -91,7 +90,7 @@ class DrawableSubmap : public QObject {
 
   ::cartographer::common::Mutex mutex_;
   ::rviz::DisplayContext* const display_context_;
-  Ogre::SceneNode* const scene_node_;
+  Ogre::SceneNode* const submap_node_;
   OgreSubmap ogre_submap_;
   ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
   ::rviz::Axes pose_axes_;

cartographer_rviz/cartographer_rviz/ogre_submap.cc

@@ -50,11 +50,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
 
 OgreSubmap::OgreSubmap(const ::cartographer::mapping::SubmapId& id,
                        Ogre::SceneManager* const scene_manager,
-                       Ogre::SceneNode* const scene_node)
+                       Ogre::SceneNode* const submap_node)
     : id_(id),
       scene_manager_(scene_manager),
-      scene_node_(scene_node),
+      submap_node_(submap_node),
-      submap_node_(scene_node_->createChildSceneNode()),
+      slice_node_(submap_node_->createChildSceneNode()),
       manual_object_(scene_manager_->createManualObject(
           kManualObjectPrefix + GetSubmapIdentifier(id))) {
   material_ = Ogre::MaterialManager::getSingleton().getByName(
@@ -66,7 +66,7 @@ OgreSubmap::OgreSubmap(const ::cartographer::mapping::SubmapId& id,
   material_->setCullingMode(Ogre::CULL_NONE);
   material_->setDepthBias(-1.f, 0.f);
   material_->setDepthWriteEnabled(false);
-  submap_node_->attachObject(manual_object_);
+  slice_node_->attachObject(manual_object_);
 }
 
 OgreSubmap::~OgreSubmap() {
@@ -75,14 +75,14 @@ OgreSubmap::~OgreSubmap() {
     Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
     texture_.setNull();
   }
-  scene_manager_->destroySceneNode(submap_node_);
+  scene_manager_->destroySceneNode(slice_node_);
   scene_manager_->destroyManualObject(manual_object_);
 }
 
 void OgreSubmap::Update(
     const ::cartographer_ros::SubmapTexture& submap_texture) {
-  submap_node_->setPosition(ToOgre(submap_texture.slice_pose.translation()));
+  slice_node_->setPosition(ToOgre(submap_texture.slice_pose.translation()));
-  submap_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation()));
+  slice_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation()));
   // The call to Ogre's loadRawData below does not work with an RG texture,
   // therefore we create an RGB one whose blue channel is always 0.
   std::vector<char> rgb;

cartographer_rviz/cartographer_rviz/ogre_submap.h

@@ -38,11 +38,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q);
 // are expected to be called from the Ogre thread.
 class OgreSubmap {
  public:
-  // Attaches a node visualizing the submap 'id' to the 'scene_node' which is
+  // Attaches a node visualizing the submap 'id' to the 'submap_node' which is
   // expected to represent the submap frame.
   OgreSubmap(const ::cartographer::mapping::SubmapId& id,
              Ogre::SceneManager* const scene_manager,
-             Ogre::SceneNode* const scene_node);
+             Ogre::SceneNode* const submap_node);
   ~OgreSubmap();
 
   OgreSubmap(const OgreSubmap&) = delete;
@@ -58,8 +58,8 @@ class OgreSubmap {
  private:
   const ::cartographer::mapping::SubmapId id_;
   Ogre::SceneManager* const scene_manager_;
-  Ogre::SceneNode* const scene_node_;
   Ogre::SceneNode* const submap_node_;
+  Ogre::SceneNode* const slice_node_;
   Ogre::ManualObject* const manual_object_;
   Ogre::TexturePtr texture_;
   Ogre::MaterialPtr material_;

cartographer_rviz/cartographer_rviz/submaps_display.cc

@@ -49,9 +49,6 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
   submap_query_service_property_ = new ::rviz::StringProperty(
       "Submap query service", kDefaultSubmapQueryServiceName,
       "Submap query service to connect to.", this, SLOT(Reset()));
-  map_frame_property_ = new ::rviz::StringProperty(
-      "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
-      this);
   tracking_frame_property_ = new ::rviz::StringProperty(
       "Tracking frame", kDefaultTrackingFrame,
       "Tracking frame, used for fading out submaps.", this);
@@ -75,7 +72,11 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
   Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
 }
 
-SubmapsDisplay::~SubmapsDisplay() { client_.shutdown(); }
+SubmapsDisplay::~SubmapsDisplay() {
+  client_.shutdown();
+  trajectories_.clear();
+  scene_manager_->destroySceneNode(map_node_);
+}
 
 void SubmapsDisplay::Reset() { reset(); }
 
@@ -86,6 +87,7 @@ void SubmapsDisplay::CreateClient() {
 
 void SubmapsDisplay::onInitialize() {
   MFDClass::onInitialize();
+  map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
   CreateClient();
 }
 
@@ -100,6 +102,8 @@ void SubmapsDisplay::reset() {
 void SubmapsDisplay::processMessage(
     const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
   ::cartographer::common::MutexLocker locker(&mutex_);
+  map_frame_ =
+      ::cartographer::common::make_unique<std::string>(msg->header.frame_id);
   // In case Cartographer node is relaunched, destroy trajectories from the
   // previous instance.
   for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
@@ -138,12 +142,11 @@ void SubmapsDisplay::processMessage(
       constexpr float kSubmapPoseAxesRadius = 0.06f;
       trajectory.emplace(id.submap_index,
                          ::cartographer::common::make_unique<DrawableSubmap>(
-                             id, context_, trajectory_category.get(),
+                             id, context_, map_node_, trajectory_category.get(),
                              visibility_all_enabled_->getBool(),
                              kSubmapPoseAxesLength, kSubmapPoseAxesRadius));
     }
-    trajectory.at(id.submap_index)
+    trajectory.at(id.submap_index)->Update(msg->header, submap_entry);
-        ->Update(msg->header, submap_entry, context_->getFrameManager());
   }
   // Remove all submaps not mentioned in the SubmapList.
   for (size_t trajectory_id = 0; trajectory_id < trajectories_.size();
@@ -162,22 +165,6 @@ void SubmapsDisplay::processMessage(
 
 void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
   ::cartographer::common::MutexLocker locker(&mutex_);
-  // Update the fading by z distance.
-  try {
-    const ::geometry_msgs::TransformStamped transform_stamped =
-        tf_buffer_.lookupTransform(map_frame_property_->getStdString(),
-                                   tracking_frame_property_->getStdString(),
-                                   ros::Time(0) /* latest */);
-    for (auto& trajectory : trajectories_) {
-      for (auto& submap_entry : trajectory.second) {
-        submap_entry.second->SetAlpha(
-            transform_stamped.transform.translation.z);
-      }
-    }
-  } catch (const tf2::TransformException& ex) {
-    ROS_WARN("Could not compute submap fading: %s", ex.what());
-  }
-
   // Schedule fetching of new submap textures.
   for (const auto& trajectory : trajectories_) {
     int num_ongoing_requests = 0;
@@ -195,6 +182,33 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
       }
     }
   }
+  if (map_frame_ == nullptr) {
+    return;
+  }
+  // Update the fading by z distance.
+  const ros::Time kLatest(0);
+  try {
+    const ::geometry_msgs::TransformStamped transform_stamped =
+        tf_buffer_.lookupTransform(
+            *map_frame_, tracking_frame_property_->getStdString(), kLatest);
+    for (auto& trajectory : trajectories_) {
+      for (auto& submap_entry : trajectory.second) {
+        submap_entry.second->SetAlpha(
+            transform_stamped.transform.translation.z);
+      }
+    }
+  } catch (const tf2::TransformException& ex) {
+    ROS_WARN("Could not compute submap fading: %s", ex.what());
+  }
+  // Update the map frame to fixed frame transform.
+  Ogre::Vector3 position;
+  Ogre::Quaternion orientation;
+  if (context_->getFrameManager()->getTransform(*map_frame_, kLatest, position,
+                                                orientation)) {
+    map_node_->setPosition(position);
+    map_node_->setOrientation(orientation);
+    context_->queueRender();
+  }
 }
 
 void SubmapsDisplay::AllEnabledToggled() {

cartographer_rviz/cartographer_rviz/submaps_display.h

@@ -19,6 +19,7 @@
 
 #include <map>
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "cartographer/common/mutex.h"
@@ -66,8 +67,9 @@ class SubmapsDisplay
   ::tf2_ros::TransformListener tf_listener_;
   ros::ServiceClient client_;
   ::rviz::StringProperty* submap_query_service_property_;
-  ::rviz::StringProperty* map_frame_property_;
+  std::unique_ptr<std::string> map_frame_;
   ::rviz::StringProperty* tracking_frame_property_;
+  Ogre::SceneNode* map_node_ = nullptr;  // Represents the map frame.
   using Trajectory = std::pair<std::unique_ptr<::rviz::Property>,
                                std::map<int, std::unique_ptr<DrawableSubmap>>>;
   std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);