Visualize both high and low res slices. (#532)

* OgreSlice has a slice_id.

This is necessary because we will have multiple OgreSlices
and Ogre requires unique names for textures and other entities.

* Visualize both high and low res slices.

Adds checkboxes to the rviz plugin to show high and low
resolution slices.
DrawableSubmap now holds a vector of OgreSlices and
decides which slices are visible.

* Corrections. Move slice visibility to OgreSlice.

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -45,6 +45,7 @@ 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;
+constexpr int kNumberOfSlicesPerSubmap = 2;
 
 }  // namespace
 
@@ -58,7 +59,6 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
       display_context_(display_context),
       submap_node_(map_node->createChildSceneNode()),
       submap_id_text_node_(submap_node_->createChildSceneNode()),
-      ogre_slice_(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)")
@@ -66,6 +66,11 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
                           .arg(id.submap_index)
                           .toStdString()),
       last_query_timestamp_(0) {
+  for (int slice_index = 0; slice_index < kNumberOfSlicesPerSubmap;
+       ++slice_index) {
+    ogre_slices_.emplace_back(::cartographer::common::make_unique<OgreSlice>(
+        id, slice_index, display_context->getSceneManager(), submap_node_));
+  }
   // 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
@@ -80,7 +85,6 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
   // 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()));
 }
 
@@ -162,19 +166,31 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) {
       target_alpha == 0.f || target_alpha == 1.f) {
     current_alpha_ = target_alpha;
   }
-  ogre_slice_.SetAlpha(current_alpha_);
+  for (auto& slice : ogre_slices_) {
+    slice->SetAlpha(current_alpha_);
+  }
   display_context_->queueRender();
 }
 
+void DrawableSubmap::SetSliceVisibility(size_t slice_index, bool visible) {
+  ogre_slices_.at(slice_index)->SetVisibility(visible);
+  ToggleVisibility();
+}
+
 void DrawableSubmap::UpdateSceneNode() {
   ::cartographer::common::MutexLocker locker(&mutex_);
-  // TODO(gaschler): Add UI feature to show all textures.
-  ogre_slice_.Update(submap_textures_->textures[0]);
+  for (size_t slice_index = 0; slice_index < ogre_slices_.size() &&
+                               slice_index < submap_textures_->textures.size();
+       ++slice_index) {
+    ogre_slices_[slice_index]->Update(submap_textures_->textures[slice_index]);
+  }
   display_context_->queueRender();
 }
 
 void DrawableSubmap::ToggleVisibility() {
-  submap_node_->setVisible(visibility_->getBool());
+  for (auto& ogre_slice : ogre_slices_) {
+    ogre_slice->UpdateOgreNodeVisibility(visibility_->getBool());
+  }
   display_context_->queueRender();
 }
 

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -70,6 +70,10 @@ class DrawableSubmap : public QObject {
   // 'current_tracking_z'.
   void SetAlpha(double current_tracking_z);
 
+  // Sets the visibility of a slice. It will be drawn if the parent submap
+  // is also visible.
+  void SetSliceVisibility(size_t slice_index, bool visible);
+
   ::cartographer::mapping::SubmapId id() const { return id_; }
   int version() const { return metadata_version_; }
   bool visibility() const { return visibility_->getBool(); }
@@ -93,7 +97,7 @@ class DrawableSubmap : public QObject {
   ::rviz::DisplayContext* const display_context_;
   Ogre::SceneNode* const submap_node_;
   Ogre::SceneNode* const submap_id_text_node_;
-  OgreSlice ogre_slice_;
+  std::vector<std::unique_ptr<OgreSlice>> ogre_slices_;
   ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
   ::rviz::Axes pose_axes_;
   ::rviz::MovableText submap_id_text_;

cartographer_rviz/cartographer_rviz/ogre_slice.cc

@@ -32,10 +32,11 @@ constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap";
 constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
 constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
 
-std::string GetSubmapIdentifier(
-    const ::cartographer::mapping::SubmapId& submap_id) {
+std::string GetSliceIdentifier(
+    const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) {
   return std::to_string(submap_id.trajectory_id) + "-" +
-         std::to_string(submap_id.submap_index);
+         std::to_string(submap_id.submap_index) + "-" +
+         std::to_string(slice_id);
 }
 
 }  // namespace
@@ -48,19 +49,20 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
   return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z());
 }
 
-OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id,
+OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id,
                      Ogre::SceneManager* const scene_manager,
                      Ogre::SceneNode* const submap_node)
     : id_(id),
+      slice_id_(slice_id),
       scene_manager_(scene_manager),
       submap_node_(submap_node),
       slice_node_(submap_node_->createChildSceneNode()),
       manual_object_(scene_manager_->createManualObject(
-          kManualObjectPrefix + GetSubmapIdentifier(id))) {
+          kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) {
   material_ = Ogre::MaterialManager::getSingleton().getByName(
       kSubmapSourceMaterialName);
-  material_ =
-      material_->clone(kSubmapMaterialPrefix + GetSubmapIdentifier(id_));
+  material_ = material_->clone(kSubmapMaterialPrefix +
+                               GetSliceIdentifier(id_, slice_id_));
   material_->setReceiveShadows(false);
   material_->getTechnique(0)->setLightingEnabled(false);
   material_->setCullingMode(Ogre::CULL_NONE);
@@ -120,7 +122,7 @@ void OgreSlice::Update(
     texture_.setNull();
   }
   const std::string texture_name =
-      kSubmapTexturePrefix + GetSubmapIdentifier(id_);
+      kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_);
   texture_ = Ogre::TextureManager::getSingleton().loadRawData(
       texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
       pixel_stream, submap_texture.width, submap_texture.height,
@@ -142,4 +144,10 @@ void OgreSlice::SetAlpha(const float alpha) {
   parameters->setNamedConstant("u_alpha", alpha);
 }
 
+void OgreSlice::SetVisibility(bool visibility) { visibility_ = visibility; }
+
+void OgreSlice::UpdateOgreNodeVisibility(bool submap_visibility) {
+  slice_node_->setVisible(submap_visibility && visibility_);
+}
+
 }  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/ogre_slice.h

@@ -40,7 +40,7 @@ class OgreSlice {
  public:
   // Attaches a node visualizing the submap 'id' to the 'submap_node' which is
   // expected to represent the submap frame.
-  OgreSlice(const ::cartographer::mapping::SubmapId& id,
+  OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id,
             Ogre::SceneManager* const scene_manager,
             Ogre::SceneNode* const submap_node);
   ~OgreSlice();
@@ -55,14 +55,24 @@ class OgreSlice {
   // Changes the opacity of the submap to 'alpha'.
   void SetAlpha(float alpha);
 
+  // Sets the local visibility of this slice.
+  void SetVisibility(bool visibility);
+
+  // Updates the SceneNode to be visible if the submap and this slice are
+  // visible.
+  void UpdateOgreNodeVisibility(bool submap_visibility);
+
  private:
+  // TODO(gaschler): Pack both ids into a struct.
   const ::cartographer::mapping::SubmapId id_;
+  const int slice_id_;
   Ogre::SceneManager* const scene_manager_;
   Ogre::SceneNode* const submap_node_;
   Ogre::SceneNode* const slice_node_;
   Ogre::ManualObject* const manual_object_;
   Ogre::TexturePtr texture_;
   Ogre::MaterialPtr material_;
+  bool visibility_ = true;
 };
 
 }  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/submaps_display.cc

@@ -52,12 +52,18 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
   tracking_frame_property_ = new ::rviz::StringProperty(
       "Tracking frame", kDefaultTrackingFrame,
       "Tracking frame, used for fading out submaps.", this);
+  slice_high_resolution_enabled_ = new ::rviz::BoolProperty(
+      "High Resolution", true, "Display high resolution slices.", this,
+      SLOT(ResolutionToggled()), this);
+  slice_low_resolution_enabled_ = new ::rviz::BoolProperty(
+      "Low Resolution", false, "Display low resolution slices.", this,
+      SLOT(ResolutionToggled()), this);
   client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
   trajectories_category_ = new ::rviz::Property(
       "Submaps", QVariant(), "List of all submaps, organized by trajectories.",
       this);
   visibility_all_enabled_ = new ::rviz::BoolProperty(
-      "All Enabled", true,
+      "All", true,
       "Whether submaps from all trajectories should be displayed or not.",
       trajectories_category_, SLOT(AllEnabledToggled()), this);
   const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
@@ -148,6 +154,10 @@ void SubmapsDisplay::processMessage(
               id, context_, map_node_, trajectory_visibility.get(),
               trajectory_visibility->getBool(), kSubmapPoseAxesLength,
               kSubmapPoseAxesRadius));
+      trajectory_submaps.at(id.submap_index)
+          ->SetSliceVisibility(0, slice_high_resolution_enabled_->getBool());
+      trajectory_submaps.at(id.submap_index)
+          ->SetSliceVisibility(1, slice_low_resolution_enabled_->getBool());
     }
     trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry);
   }
@@ -223,6 +233,18 @@ void SubmapsDisplay::AllEnabledToggled() {
   }
 }
 
+void SubmapsDisplay::ResolutionToggled() {
+  ::cartographer::common::MutexLocker locker(&mutex_);
+  for (auto& trajectory : trajectories_) {
+    for (auto& submap_entry : trajectory->submaps) {
+      submap_entry.second->SetSliceVisibility(
+          0, slice_high_resolution_enabled_->getBool());
+      submap_entry.second->SetSliceVisibility(
+          1, slice_low_resolution_enabled_->getBool());
+    }
+  }
+}
+
 void Trajectory::AllEnabledToggled() {
   const bool visible = visibility->getBool();
   for (auto& submap_entry : submaps) {

cartographer_rviz/cartographer_rviz/submaps_display.h

@@ -32,8 +32,9 @@
 
 namespace cartographer_rviz {
 
-// This should be a private class in SubmapsDisplay, unfortunately, QT does not
-// allow for this.
+// TODO(gaschler): This should be a private class in SubmapsDisplay,
+// unfortunately, QT does not allow for this. Move the logic out of the struct
+// and use just one slot for all changes.
 struct Trajectory : public QObject {
   Q_OBJECT
 
@@ -67,6 +68,7 @@ class SubmapsDisplay
  private Q_SLOTS:
   void Reset();
   void AllEnabledToggled();
+  void ResolutionToggled();
 
  private:
   void CreateClient();
@@ -87,6 +89,8 @@ class SubmapsDisplay
   Ogre::SceneNode* map_node_ = nullptr;  // Represents the map frame.
   std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
   ::cartographer::common::Mutex mutex_;
+  ::rviz::BoolProperty* slice_high_resolution_enabled_;
+  ::rviz::BoolProperty* slice_low_resolution_enabled_;
   ::rviz::Property* trajectories_category_;
   ::rviz::BoolProperty* visibility_all_enabled_;
 };