Display submap poses in Submaps plugin (#416)

- simplify code around toggling submap visibility
- pass DisplayContext to DrawableSubmap
- add parent scene node for both submap and axes
- remove unnecessary const

Fixes #405.

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -64,15 +64,21 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
 
 }  // namespace
 
-DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
-                               Ogre::SceneManager* const scene_manager,
-                               ::rviz::Property* submap_category,
-                               const bool visible)
+DrawableSubmap::DrawableSubmap(
+    const ::cartographer::mapping::SubmapId& id,
+    ::rviz::DisplayContext* const display_context,
+    ::rviz::Property* const submap_category, const bool visible,
+    const float pose_axes_length, const float pose_axes_radius)
     : id_(id),
-      scene_manager_(scene_manager),
-      scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()),
-      manual_object_(scene_manager->createManualObject(
+      display_context_(display_context),
+      scene_node_(display_context->getSceneManager()
+                      ->getRootSceneNode()
+                      ->createChildSceneNode()),
+      submap_node_(scene_node_->createChildSceneNode()),
+      manual_object_(display_context->getSceneManager()->createManualObject(
           kManualObjectPrefix + GetSubmapIdentifier(id))),
+      pose_axes_(display_context->getSceneManager(), scene_node_,
+                 pose_axes_length, pose_axes_radius),
       last_query_timestamp_(0) {
   material_ = Ogre::MaterialManager::getSingleton().getByName(
       kSubmapSourceMaterialName);
@@ -90,9 +96,8 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
   visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>(
       "" /* title */, visible, "" /* description */, submap_category,
       SLOT(ToggleVisibility()), this);
-  if (visible) {
-    scene_node_->attachObject(manual_object_);
-  }
+  submap_node_->attachObject(manual_object_);
+  scene_node_->setVisible(visible);
   connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
 }
 
@@ -107,8 +112,9 @@ DrawableSubmap::~DrawableSubmap() {
     Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
     texture_.setNull();
   }
-  scene_manager_->destroySceneNode(scene_node_);
-  scene_manager_->destroyManualObject(manual_object_);
+  display_context_->getSceneManager()->destroySceneNode(submap_node_);
+  display_context_->getSceneManager()->destroySceneNode(scene_node_);
+  display_context_->getSceneManager()->destroyManualObject(manual_object_);
 }
 
 void DrawableSubmap::Update(
@@ -250,8 +256,11 @@ void DrawableSubmap::UpdateTransform() {
   CHECK(submap_texture_ != nullptr);
   const ::cartographer::transform::Rigid3d pose =
       pose_ * submap_texture_->slice_pose;
-  scene_node_->setPosition(ToOgre(pose.translation()));
-  scene_node_->setOrientation(ToOgre(pose.rotation()));
+  submap_node_->setPosition(ToOgre(pose.translation()));
+  submap_node_->setOrientation(ToOgre(pose.rotation()));
+  pose_axes_.setPosition(ToOgre(pose_.translation()));
+  pose_axes_.setOrientation(ToOgre(pose_.rotation()));
+  display_context_->queueRender();
 }
 
 float DrawableSubmap::UpdateAlpha(const float target_alpha) {
@@ -263,15 +272,7 @@ float DrawableSubmap::UpdateAlpha(const float target_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_);
-    }
-  }
+  scene_node_->setVisible(visibility_->getBool());
 }
 
 }  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -38,6 +38,7 @@
 #include "ros/ros.h"
 #include "rviz/display_context.h"
 #include "rviz/frame_manager.h"
+#include "rviz/ogre_helpers/axes.h"
 #include "rviz/properties/bool_property.h"
 
 namespace cartographer_rviz {
@@ -48,10 +49,10 @@ class DrawableSubmap : public QObject {
   Q_OBJECT
 
  public:
-  // The 'scene_manager' is the Ogre scene manager to which to add a node.
   DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id,
-                 Ogre::SceneManager* scene_manager,
-                 ::rviz::Property* submap_category, const bool visible);
+                 ::rviz::DisplayContext* display_context,
+                 ::rviz::Property* submap_category, bool visible,
+                 float pose_axes_length, float pose_axes_radius);
   ~DrawableSubmap() override;
   DrawableSubmap(const DrawableSubmap&) = delete;
   DrawableSubmap& operator=(const DrawableSubmap&) = delete;
@@ -96,12 +97,14 @@ class DrawableSubmap : public QObject {
   const ::cartographer::mapping::SubmapId id_;
 
   ::cartographer::common::Mutex mutex_;
-  Ogre::SceneManager* const scene_manager_;
+  ::rviz::DisplayContext* const display_context_;
   Ogre::SceneNode* const scene_node_;
-  Ogre::ManualObject* manual_object_;
+  Ogre::SceneNode* const submap_node_;
+  Ogre::ManualObject* const manual_object_;
   Ogre::TexturePtr texture_;
   Ogre::MaterialPtr material_;
   ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
+  ::rviz::Axes pose_axes_;
   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_rviz/cartographer_rviz/submaps_display.cc

@@ -133,11 +133,14 @@ void SubmapsDisplay::processMessage(
     auto& trajectory_category = trajectories_[id.trajectory_id].first;
     auto& trajectory = trajectories_[id.trajectory_id].second;
     if (trajectory.count(id.submap_index) == 0) {
-      trajectory.emplace(
-          id.submap_index,
-          ::cartographer::common::make_unique<DrawableSubmap>(
-              id, context_->getSceneManager(), trajectory_category.get(),
-              visibility_all_enabled_->getBool()));
+      // TODO(ojura): Add RViz properties for adjusting submap pose axes
+      constexpr float kSubmapPoseAxesLength = 0.3f;
+      constexpr float kSubmapPoseAxesRadius = 0.06f;
+      trajectory.emplace(id.submap_index,
+                         ::cartographer::common::make_unique<DrawableSubmap>(
+                             id, context_, trajectory_category.get(),
+                             visibility_all_enabled_->getBool(),
+                             kSubmapPoseAxesLength, kSubmapPoseAxesRadius));
     }
     trajectory.at(id.submap_index)
         ->Update(msg->header, submap_entry, context_->getFrameManager());