Add visibility toggling of individual submaps in cartographer_rviz

- Add a variable list object to the submaps plugin, containing visibility
checkboxes for individual submaps. Each DrawableSubmap creates and
manages its visibility property checkbox object. Submaps are hidden by
detaching the Ogre manual object from the Ogre scene node. 
- Fetch if the submap version number is different, possibly lower
  (meaning Cartographer was restarted)

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -25,6 +25,7 @@
 #include "Eigen/Geometry"
 #include "OgreGpuProgramParams.h"
 #include "OgreImage.h"
+#include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "eigen_conversions/eigen_msg.h"
@@ -54,7 +55,9 @@ std::string GetSubmapIdentifier(const int trajectory_id,
 }  // namespace
 
 DrawableSubmap::DrawableSubmap(const int trajectory_id, const int submap_index,
-                               Ogre::SceneManager* const scene_manager)
+                               Ogre::SceneManager* const scene_manager,
+                               ::rviz::Property* submap_category,
+                               const bool visible)
     : trajectory_id_(trajectory_id),
       submap_index_(submap_index),
       scene_manager_(scene_manager),
@@ -73,7 +76,16 @@ DrawableSubmap::DrawableSubmap(const int trajectory_id, const int submap_index,
   material_->setCullingMode(Ogre::CULL_NONE);
   material_->setDepthBias(-1.f, 0.f);
   material_->setDepthWriteEnabled(false);
-  scene_node_->attachObject(manual_object_);
+  // DrawableSubmap creates and manages its visibility property object
+  // (a unique_ptr is needed because the Qt parent of the visibility
+  // property is the submap_category object - the BoolProperty needs
+  // to be destroyed along with the DrawableSubmap)
+  visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>(
+      "" /* title */, visible, "" /* description */, submap_category,
+      SLOT(ToggleVisibility()), this);
+  if (visible) {
+    scene_node_->attachObject(manual_object_);
+  }
   connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
 }
 
@@ -110,11 +122,20 @@ void DrawableSubmap::Update(
     // for this submap.
     UpdateTransform();
   }
+  visibility_->setName(
+      QString("%1.%2").arg(submap_index_).arg(metadata_version_));
+  visibility_->setDescription(
+      QString("Toggle visibility of this individual submap.<br><br>"
+              "Trajectory %1, submap %2, submap version %3")
+          .arg(trajectory_id_)
+          .arg(submap_index_)
+          .arg(metadata_version_));
 }
 
 bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
   ::cartographer::common::MutexLocker locker(&mutex_);
-  const bool newer_version_available = texture_version_ < metadata_version_;
+  // Received metadata version can also be lower - if we restarted Cartographer
+  const bool newer_version_available = texture_version_ != metadata_version_;
   const std::chrono::milliseconds now =
       std::chrono::duration_cast<std::chrono::milliseconds>(
           std::chrono::system_clock::now().time_since_epoch());
@@ -244,4 +265,16 @@ float DrawableSubmap::UpdateAlpha(const float target_alpha) {
   return current_alpha_;
 }
 
+void DrawableSubmap::ToggleVisibility() {
+  if (visibility_->getBool()) {
+    if (scene_node_->numAttachedObjects() == 0) {
+      scene_node_->attachObject(manual_object_);
+    }
+  } else {
+    if (scene_node_->numAttachedObjects() > 0) {
+      scene_node_->detachObject(manual_object_);
+    }
+  }
+}
+
 }  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -34,6 +34,7 @@
 #include "ros/ros.h"
 #include "rviz/display_context.h"
 #include "rviz/frame_manager.h"
+#include "rviz/properties/bool_property.h"
 
 namespace cartographer_rviz {
 
@@ -46,7 +47,8 @@ class DrawableSubmap : public QObject {
   // Each submap is identified by a 'trajectory_id' plus a 'submap_index'.
   // 'scene_manager' is the Ogre scene manager to which to add a node.
   DrawableSubmap(int trajectory_id, int submap_index,
-                 Ogre::SceneManager* scene_manager);
+                 Ogre::SceneManager* scene_manager,
+                 ::rviz::Property* submap_category, const bool visible);
   ~DrawableSubmap() override;
   DrawableSubmap(const DrawableSubmap&) = delete;
   DrawableSubmap& operator=(const DrawableSubmap&) = delete;
@@ -68,6 +70,13 @@ class DrawableSubmap : public QObject {
   // 'current_tracking_z'.
   void SetAlpha(double current_tracking_z);
 
+  int submap_index() const { return submap_index_; }
+  int trajectory_id() const { return trajectory_id_; }
+  bool visibility() const { return visibility_->getBool(); }
+  void set_visibility(const bool visibility) {
+    visibility_->setBool(visibility);
+  }
+
  Q_SIGNALS:
   // RPC request succeeded.
   void RequestSucceeded();
@@ -75,6 +84,7 @@ class DrawableSubmap : public QObject {
  private Q_SLOTS:
   // Callback when an rpc request succeeded.
   void UpdateSceneNode();
+  void ToggleVisibility();
 
  private:
   void UpdateTransform();
@@ -100,6 +110,7 @@ class DrawableSubmap : public QObject {
   std::future<void> rpc_request_future_;
   ::cartographer_ros_msgs::SubmapQuery::Response response_ GUARDED_BY(mutex_);
   float current_alpha_ = 0.f;
+  std::unique_ptr<::rviz::BoolProperty> visibility_;
 };
 
 }  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/submaps_display.cc

@@ -27,6 +27,7 @@
 #include "ros/ros.h"
 #include "rviz/display_context.h"
 #include "rviz/frame_manager.h"
+#include "rviz/properties/bool_property.h"
 #include "rviz/properties/string_property.h"
 
 namespace cartographer_rviz {
@@ -54,6 +55,13 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
       "Tracking frame", kDefaultTrackingFrame,
       "Tracking frame, used for fading out submaps.", this);
   client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
+  submaps_category_ = new ::rviz::Property(
+      "Submaps", QVariant(), "List of all submaps, organized by trajectories.",
+      this);
+  visibility_all_enabled_ = new ::rviz::BoolProperty(
+      "All Enabled", true,
+      "Whether all the submaps should be displayed or not.", submaps_category_,
+      SLOT(AllEnabledToggled()), this);
   const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
   Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
       package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
@@ -91,20 +99,40 @@ void SubmapsDisplay::reset() {
 void SubmapsDisplay::processMessage(
     const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
   ::cartographer::common::MutexLocker locker(&mutex_);
+  // In case Cartographer node is relaunched, destroy
+  // trajectories from the previous instance
+  if (msg->trajectory.size() < trajectories_.size()) {
+    trajectories_.clear();
+  }
   for (size_t trajectory_id = 0; trajectory_id < msg->trajectory.size();
        ++trajectory_id) {
     if (trajectory_id >= trajectories_.size()) {
-      trajectories_.emplace_back();
+      // When a trajectory is destroyed, it also needs to delete its rviz
+      // Property object, so we use a unique_ptr for it
+      trajectories_.push_back(Trajectory(
+          ::cartographer::common::make_unique<::rviz::Property>(
+              QString("Trajectory %1").arg(trajectory_id), QVariant(),
+              QString("List of all submaps in Trajectory %1.")
+                  .arg(trajectory_id),
+              submaps_category_),
+          std::vector<std::unique_ptr<DrawableSubmap>>()));
     }
-    auto& trajectory = trajectories_[trajectory_id];
+    auto& trajectory_category = trajectories_[trajectory_id].first;
+    auto& trajectory = trajectories_[trajectory_id].second;
     const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
         msg->trajectory[trajectory_id].submap;
+    // Same as above, destroy the whole trajectory if we detect that
+    // we have more submaps than we should
+    if (submap_entries.size() < trajectory.size()) {
+      trajectory.clear();
+    }
     for (size_t submap_index = 0; submap_index < submap_entries.size();
          ++submap_index) {
       if (submap_index >= trajectory.size()) {
         trajectory.push_back(
             ::cartographer::common::make_unique<DrawableSubmap>(
-                trajectory_id, submap_index, context_->getSceneManager()));
+                trajectory_id, submap_index, context_->getSceneManager(),
+                trajectory_category.get(), visibility_all_enabled_->getBool()));
       }
       trajectory[submap_index]->Update(msg->header,
                                        submap_entries[submap_index],
@@ -122,7 +150,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
                                    tracking_frame_property_->getStdString(),
                                    ros::Time(0) /* latest */);
     for (auto& trajectory : trajectories_) {
-      for (auto& submap : trajectory) {
+      for (auto& submap : trajectory.second) {
         submap->SetAlpha(transform_stamped.transform.translation.z);
       }
     }
@@ -133,22 +161,32 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
   // Schedule fetching of new submap textures.
   for (const auto& trajectory : trajectories_) {
     int num_ongoing_requests = 0;
-    for (const auto& submap : trajectory) {
+    for (const auto& submap : trajectory.second) {
       if (submap->QueryInProgress()) {
         ++num_ongoing_requests;
       }
     }
-    for (int submap_index = static_cast<int>(trajectory.size()) - 1;
+    for (int submap_index = static_cast<int>(trajectory.second.size()) - 1;
          submap_index >= 0 &&
          num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
          --submap_index) {
-      if (trajectory[submap_index]->MaybeFetchTexture(&client_)) {
+      if (trajectory.second[submap_index]->MaybeFetchTexture(&client_)) {
         ++num_ongoing_requests;
       }
     }
   }
 }
 
+void SubmapsDisplay::AllEnabledToggled() {
+  ::cartographer::common::MutexLocker locker(&mutex_);
+  const bool visibility = visibility_all_enabled_->getBool();
+  for (auto& trajectory : trajectories_) {
+    for (auto& submap : trajectory.second) {
+      submap->set_visibility(visibility);
+    }
+  }
+}
+
 }  // namespace cartographer_rviz
 
 PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz::Display)

cartographer_rviz/cartographer_rviz/submaps_display.h

@@ -49,6 +49,7 @@ class SubmapsDisplay
 
  private Q_SLOTS:
   void Reset();
+  void AllEnabledToggled();
 
  private:
   void CreateClient();
@@ -66,9 +67,12 @@ class SubmapsDisplay
   ::rviz::StringProperty* submap_query_service_property_;
   ::rviz::StringProperty* map_frame_property_;
   ::rviz::StringProperty* tracking_frame_property_;
-  using Trajectory = std::vector<std::unique_ptr<DrawableSubmap>>;
+  using Trajectory = std::pair<std::unique_ptr<::rviz::Property>,
+                               std::vector<std::unique_ptr<DrawableSubmap>>>;
   std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);
   ::cartographer::common::Mutex mutex_;
+  ::rviz::Property* submaps_category_;
+  ::rviz::BoolProperty* visibility_all_enabled_;
 };
 
 }  // namespace cartographer_rviz