From a40357b61d61e04543f911f7246975c7f98b874d Mon Sep 17 00:00:00 2001
From: Jihoon Lee <jihoonlee.in@gmail.com>
Date: Fri, 28 Jul 2017 16:40:25 +0200
Subject: [PATCH] Show submap ID in rviz (#447)

---
 .../cartographer_rviz/drawable_submap.cc        | 17 +++++++++++++++++
 .../cartographer_rviz/drawable_submap.h         |  3 +++
 2 files changed, 20 insertions(+)

diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc
index 97a8489..9d275e8 100644
--- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc
+++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc
@@ -42,6 +42,10 @@ constexpr double kFadeOutStartDistanceInMeters = 1.;
 constexpr double kFadeOutDistanceInMeters = 2.;
 constexpr float kAlphaUpdateThreshold = 0.2f;
 
+const Ogre::ColourValue kSubmapIdColor(Ogre::ColourValue::Red);
+const Eigen::Vector3d kSubmapIdPosition(0.0, 0.0, 0.3);
+constexpr float kSubmapIdCharHeight = 0.2f;
+
 }  // namespace
 
 DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
@@ -53,9 +57,14 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
     : id_(id),
       display_context_(display_context),
       submap_node_(map_node->createChildSceneNode()),
+      submap_id_text_node_(submap_node_->createChildSceneNode()),
       ogre_submap_(id, display_context->getSceneManager(), submap_node_),
       pose_axes_(display_context->getSceneManager(), submap_node_,
                  pose_axes_length, pose_axes_radius),
+      submap_id_text_(QString("(%1,%2)")
+                          .arg(id.trajectory_id)
+                          .arg(id.submap_index)
+                          .toStdString()),
       last_query_timestamp_(0) {
   // DrawableSubmap creates and manages its visibility property object
   // (a unique_ptr is needed because the Qt parent of the visibility
@@ -64,6 +73,13 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
   visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>(
       "" /* title */, visible, "" /* description */, submap_category,
       SLOT(ToggleVisibility()), this);
+  submap_id_text_.setCharacterHeight(kSubmapIdCharHeight);
+  submap_id_text_.setColor(kSubmapIdColor);
+  submap_id_text_.setTextAlignment(::rviz::MovableText::H_CENTER,
+                                   ::rviz::MovableText::V_ABOVE);
+  // TODO(jihoonl): Make it toggleable.
+  submap_id_text_node_->setPosition(ToOgre(kSubmapIdPosition));
+  submap_id_text_node_->attachObject(&submap_id_text_);
   submap_node_->setVisible(visible);
   connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
 }
@@ -75,6 +91,7 @@ DrawableSubmap::~DrawableSubmap() {
     rpc_request_future_.wait();
   }
   display_context_->getSceneManager()->destroySceneNode(submap_node_);
+  display_context_->getSceneManager()->destroySceneNode(submap_id_text_node_);
 }
 
 void DrawableSubmap::Update(
diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h
index 82e8153..fa82020 100644
--- a/cartographer_rviz/cartographer_rviz/drawable_submap.h
+++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h
@@ -35,6 +35,7 @@
 #include "rviz/display_context.h"
 #include "rviz/frame_manager.h"
 #include "rviz/ogre_helpers/axes.h"
+#include "rviz/ogre_helpers/movable_text.h"
 #include "rviz/properties/bool_property.h"
 
 namespace cartographer_rviz {
@@ -91,9 +92,11 @@ class DrawableSubmap : public QObject {
   ::cartographer::common::Mutex mutex_;
   ::rviz::DisplayContext* const display_context_;
   Ogre::SceneNode* const submap_node_;
+  Ogre::SceneNode* const submap_id_text_node_;
   OgreSubmap ogre_submap_;
   ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
   ::rviz::Axes pose_axes_;
+  ::rviz::MovableText submap_id_text_;
   std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
   bool query_in_progress_ = false GUARDED_BY(mutex_);
   int metadata_version_ = -1 GUARDED_BY(mutex_);