Fix the submap transformation in the RViz plugin. (#13)

Removes the SceneManagerListener and instead computes the transform
when a new submap list is received. The second part of the transform
is also updated when a new texture is received.

Also simplifies the drawing code a bit by using a triangle strip.

cartographer_ros/src/drawable_submap.cc

@@ -22,8 +22,6 @@
 #include <cartographer_ros_msgs/SubmapQuery.h>
 #include <eigen_conversions/eigen_msg.h>
 #include <ros/ros.h>
-#include <rviz/display_context.h>
-#include <rviz/frame_manager.h>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
@@ -38,7 +36,6 @@ namespace rviz {
 namespace {
 
 constexpr std::chrono::milliseconds kMinQueryDelayInMs(250);
-constexpr char kMapFrame[] = "/map";
 constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
 constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
 constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
@@ -87,10 +84,25 @@ DrawableSubmap::~DrawableSubmap() {
 }
 
 void DrawableSubmap::Update(
-    const ::cartographer_ros_msgs::SubmapEntry& metadata) {
+    const ::std_msgs::Header& header,
+    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_);
-  tf::poseMsgToEigen(metadata.pose, submap_pose_);
+  position_ = position;
+  orientation_ = orientation;
+  submap_z_ = metadata.pose.position.z;
   metadata_version_ = metadata.submap_version;
+  if (texture_version_ != -1) {
+    // We have to update the transform since we are already displaying a texture
+    // for this submap.
+    UpdateTransform();
+  }
 }
 
 bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
@@ -106,11 +118,6 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
   }
   query_in_progress_ = true;
   last_query_timestamp_ = now;
-  QuerySubmap(client);
-  return true;
-}
-
-void DrawableSubmap::QuerySubmap(ros::ServiceClient* const client) {
   rpc_request_future_ = std::async(std::launch::async, [this, client]() {
     ::cartographer_ros_msgs::SubmapQuery srv;
     srv.request.submap_id = submap_id_;
@@ -126,6 +133,7 @@ void DrawableSubmap::QuerySubmap(ros::ServiceClient* const client) {
       query_in_progress_ = false;
     }
   });
+  return true;
 }
 
 bool DrawableSubmap::QueryInProgress() {
@@ -133,15 +141,26 @@ bool DrawableSubmap::QueryInProgress() {
   return query_in_progress_;
 }
 
+void DrawableSubmap::SetAlpha(const double current_tracking_z) {
+  const double distance_z = std::abs(submap_z_ - current_tracking_z);
+  const double fade_distance =
+      std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
+  const float alpha = static_cast<float>(
+      std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters));
+
+  const Ogre::GpuProgramParametersSharedPtr parameters =
+      material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
+  parameters->setNamedConstant("u_alpha", UpdateAlpha(alpha));
+}
+
 void DrawableSubmap::UpdateSceneNode() {
   ::cartographer::common::MutexLocker locker(&mutex_);
   texture_version_ = response_.submap_version;
   std::string compressed_cells(response_.cells.begin(), response_.cells.end());
   std::string cells;
   ::cartographer::common::FastGunzipString(compressed_cells, &cells);
-  Eigen::Affine3d slice_pose;
-  tf::poseMsgToEigen(response_.slice_pose, slice_pose);
-  tf::poseEigenToMsg(submap_pose_ * slice_pose, transformed_pose_);
+  tf::poseMsgToEigen(response_.slice_pose, slice_pose_);
+  UpdateTransform();
   query_in_progress_ = false;
   // 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.
@@ -159,43 +178,20 @@ void DrawableSubmap::UpdateSceneNode() {
   manual_object_->clear();
   const float metric_width = response_.resolution * response_.width;
   const float metric_height = response_.resolution * response_.height;
-
   manual_object_->begin(material_->getName(),
-                        Ogre::RenderOperation::OT_TRIANGLE_LIST);
-  {
-    {
-      // Bottom left
-      manual_object_->position(-metric_height, 0.0f, 0.0f);
-      manual_object_->textureCoord(0.0f, 1.0f);
-      manual_object_->normal(0.0f, 0.0f, 1.0f);
-
-      // Bottom right
-      manual_object_->position(-metric_height, -metric_width, 0.0f);
-      manual_object_->textureCoord(1.0f, 1.0f);
-      manual_object_->normal(0.0f, 0.0f, 1.0f);
-
-      // Top left
-      manual_object_->position(0.0f, 0.0f, 0.0f);
-      manual_object_->textureCoord(0.0f, 0.0f);
-      manual_object_->normal(0.0f, 0.0f, 1.0f);
-
-      // Top left
-      manual_object_->position(0.0f, 0.0f, 0.0f);
-      manual_object_->textureCoord(0.0f, 0.0f);
-      manual_object_->normal(0.0f, 0.0f, 1.0f);
-
-      // Bottom right
-      manual_object_->position(-metric_height, -metric_width, 0.0f);
-      manual_object_->textureCoord(1.0f, 1.0f);
-      manual_object_->normal(0.0f, 0.0f, 1.0f);
-
-      // Top right
-      manual_object_->position(0.0f, -metric_width, 0.0f);
-      manual_object_->textureCoord(1.0f, 0.0f);
-      manual_object_->normal(0.0f, 0.0f, 1.0f);
-    }
-  }
-
+                        Ogre::RenderOperation::OT_TRIANGLE_STRIP);
+  // Bottom left
+  manual_object_->position(-metric_height, 0.0f, 0.0f);
+  manual_object_->textureCoord(0.0f, 1.0f);
+  // Bottom right
+  manual_object_->position(-metric_height, -metric_width, 0.0f);
+  manual_object_->textureCoord(1.0f, 1.0f);
+  // Top left
+  manual_object_->position(0.0f, 0.0f, 0.0f);
+  manual_object_->textureCoord(0.0f, 0.0f);
+  // Top right
+  manual_object_->position(0.0f, -metric_width, 0.0f);
+  manual_object_->textureCoord(1.0f, 0.0f);
   manual_object_->end();
 
   Ogre::DataStreamPtr pixel_stream;
@@ -222,26 +218,15 @@ void DrawableSubmap::UpdateSceneNode() {
   texture_unit->setTextureFiltering(Ogre::TFO_NONE);
 }
 
-void DrawableSubmap::Transform(::rviz::FrameManager* const frame_manager) {
-  Ogre::Vector3 position;
-  Ogre::Quaternion orientation;
-  frame_manager->transform(kMapFrame, ros::Time(0) /* latest */,
-                           transformed_pose_, position, orientation);
-  scene_node_->setPosition(position);
-  scene_node_->setOrientation(orientation);
-}
-
-void DrawableSubmap::SetAlpha(const double current_tracking_z) {
-  const double distance_z =
-      std::abs(submap_pose_.translation().z() - current_tracking_z);
-  const double fade_distance =
-      std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
-  const float alpha =
-      (float)std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters);
-
-  const Ogre::GpuProgramParametersSharedPtr parameters =
-      material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
-  parameters->setNamedConstant("u_alpha", UpdateAlpha(alpha));
+void DrawableSubmap::UpdateTransform() {
+  const Eigen::Quaterniond quaternion(slice_pose_.rotation());
+  const Ogre::Quaternion slice_rotation(quaternion.w(), quaternion.x(),
+                                        quaternion.y(), quaternion.z());
+  const Ogre::Vector3 slice_translation(slice_pose_.translation().x(),
+                                        slice_pose_.translation().y(),
+                                        slice_pose_.translation().z());
+  scene_node_->setPosition(orientation_ * slice_translation + position_);
+  scene_node_->setOrientation(orientation_ * slice_rotation);
 }
 
 float DrawableSubmap::UpdateAlpha(const float target_alpha) {

cartographer_ros/src/drawable_submap.h

@@ -19,14 +19,17 @@
 
 #include <OgreManualObject.h>
 #include <OgreMaterial.h>
+#include <OgreQuaternion.h>
 #include <OgreSceneManager.h>
 #include <OgreSceneNode.h>
 #include <OgreTexture.h>
+#include <OgreVector3.h>
 #include <cartographer/common/mutex.h>
 #include <cartographer_ros_msgs/SubmapEntry.h>
 #include <cartographer_ros_msgs/SubmapQuery.h>
 #include <ros/ros.h>
 #include <rviz/display_context.h>
+#include <rviz/frame_manager.h>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
@@ -51,7 +54,9 @@ 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 ::cartographer_ros_msgs::SubmapEntry& metadata);
+  void Update(const ::std_msgs::Header& header,
+              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.
@@ -60,10 +65,6 @@ class DrawableSubmap : public QObject {
   // Returns whether an RPC is in progress.
   bool QueryInProgress();
 
-  // Transforms the scene node for this submap before being rendered onto a
-  // texture.
-  void Transform(::rviz::FrameManager* frame_manager);
-
   // Sets the alpha of the submap taking into account its slice height and the
   // 'current_tracking_z'.
   void SetAlpha(double current_tracking_z);
@@ -77,7 +78,7 @@ class DrawableSubmap : public QObject {
   void UpdateSceneNode();
 
  private:
-  void QuerySubmap(ros::ServiceClient* client);
+  void UpdateTransform();
   float UpdateAlpha(float target_alpha);
 
   const int submap_id_;
@@ -89,8 +90,10 @@ class DrawableSubmap : public QObject {
   Ogre::ManualObject* manual_object_;
   Ogre::TexturePtr texture_;
   Ogre::MaterialPtr material_;
-  Eigen::Affine3d submap_pose_ GUARDED_BY(mutex_);
-  geometry_msgs::Pose transformed_pose_ GUARDED_BY(mutex_);
+  double submap_z_ = 0. GUARDED_BY(mutex_);
+  Ogre::Vector3 position_ GUARDED_BY(mutex_);
+  Ogre::Quaternion orientation_ GUARDED_BY(mutex_);
+  Eigen::Affine3d slice_pose_ GUARDED_BY(mutex_);
   std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
   bool query_in_progress_ = false GUARDED_BY(mutex_);
   int metadata_version_ = -1 GUARDED_BY(mutex_);

cartographer_ros/src/submaps_display.cc

@@ -17,7 +17,6 @@
 #include "submaps_display.h"
 
 #include <OgreResourceGroupManager.h>
-#include <OgreSceneManager.h>
 #include <cartographer/common/make_unique.h>
 #include <cartographer/common/mutex.h>
 #include <cartographer_ros_msgs/SubmapList.h>
@@ -28,7 +27,6 @@
 #include <ros/ros.h>
 #include <rviz/display_context.h>
 #include <rviz/frame_manager.h>
-#include <rviz/properties/ros_topic_property.h>
 #include <rviz/properties/string_property.h>
 
 #include "node_constants.h"
@@ -47,9 +45,7 @@ constexpr char kDefaultTrackingFrame[] = "base_link";
 
 }  // namespace
 
-SubmapsDisplay::SubmapsDisplay()
-    : scene_manager_listener_([this]() { UpdateTransforms(); }),
-      tf_listener_(tf_buffer_) {
+SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
   submap_query_service_property_ = new ::rviz::StringProperty(
       "Submap query service",
       QString("/cartographer/") + kSubmapQueryServiceName,
@@ -84,7 +80,6 @@ void SubmapsDisplay::CreateClient() {
 
 void SubmapsDisplay::onInitialize() {
   MFDClass::onInitialize();
-  scene_manager_->addListener(&scene_manager_listener_);
   CreateClient();
 }
 
@@ -113,7 +108,8 @@ void SubmapsDisplay::processMessage(
             ::cartographer::common::make_unique<DrawableSubmap>(
                 submap_id, trajectory_id, context_->getSceneManager()));
       }
-      trajectory[submap_id]->Update(submap_entries[submap_id]);
+      trajectory[submap_id]->Update(msg->header, submap_entries[submap_id],
+                                    context_->getFrameManager());
     }
   }
 }
@@ -154,15 +150,6 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
   }
 }
 
-void SubmapsDisplay::UpdateTransforms() {
-  ::cartographer::common::MutexLocker locker(&mutex_);
-  for (auto& trajectory : trajectories_) {
-    for (auto& submap : trajectory) {
-      submap->Transform(context_->getFrameManager());
-    }
-  }
-}
-
 }  // namespace rviz
 }  // namespace cartographer_ros
 

cartographer_ros/src/submaps_display.h

@@ -17,18 +17,11 @@
 #ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_
 #define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_
 
-#include <OgreMaterial.h>
-#include <OgreSceneManager.h>
-#include <OgreSharedPtr.h>
-#include <OgreTexture.h>
-#include <OgreVector3.h>
 #include <cartographer/common/mutex.h>
 #include <cartographer/common/port.h>
 #include <cartographer_ros_msgs/SubmapList.h>
-#include <nav_msgs/MapMetaData.h>
-#include <ros/time.h>
 #include <rviz/message_filter_display.h>
-#include <tf/tfMessage.h>
+#include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
@@ -60,18 +53,6 @@ class SubmapsDisplay
   void Reset();
 
  private:
-  class SceneManagerListener : public Ogre::SceneManager::Listener {
-   public:
-    SceneManagerListener(std::function<void()> callback)
-        : callback_(callback) {}
-    void preUpdateSceneGraph(Ogre::SceneManager* source, Ogre::Camera* camera) {
-      callback_();
-    }
-
-   private:
-    std::function<void()> callback_;
-  };
-
   void CreateClient();
 
   void onInitialize() override;
@@ -80,9 +61,6 @@ class SubmapsDisplay
       const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) override;
   void update(float wall_dt, float ros_dt) override;
 
-  void UpdateTransforms();
-
-  SceneManagerListener scene_manager_listener_;
   ::tf2_ros::Buffer tf_buffer_;
   ::tf2_ros::TransformListener tf_listener_;
   ros::ServiceClient client_;