Extract the Ogre related submap code into a separate class. (#444)

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -23,8 +23,6 @@
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
-#include "OgreGpuProgramParams.h"
-#include "OgreImage.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer_ros/msg_conversion.h"
@@ -37,10 +35,6 @@ namespace cartographer_rviz {
 namespace {
 
 constexpr std::chrono::milliseconds kMinQueryDelayInMs(250);
-constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
-constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
-constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
-constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap";
 
 // Distance before which the submap will be shown at full opacity, and distance
 // over which the submap will then fade out.
@@ -48,20 +42,6 @@ constexpr double kFadeOutStartDistanceInMeters = 1.;
 constexpr double kFadeOutDistanceInMeters = 2.;
 constexpr float kAlphaUpdateThreshold = 0.2f;
 
-std::string GetSubmapIdentifier(
-    const ::cartographer::mapping::SubmapId& submap_id) {
-  return std::to_string(submap_id.trajectory_id) + "-" +
-         std::to_string(submap_id.submap_index);
-}
-
-Ogre::Vector3 ToOgre(const Eigen::Vector3d& v) {
-  return Ogre::Vector3(v.x(), v.y(), v.z());
-}
-
-Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
-  return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z());
-}
-
 }  // namespace
 
 DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
@@ -74,21 +54,10 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
       scene_node_(display_context->getSceneManager()
                       ->getRootSceneNode()
                       ->createChildSceneNode()),
-      submap_node_(scene_node_->createChildSceneNode()),
+      ogre_submap_(id, display_context->getSceneManager(), scene_node_),
-      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);
-  material_ =
-      material_->clone(kSubmapMaterialPrefix + GetSubmapIdentifier(id_));
-  material_->setReceiveShadows(false);
-  material_->getTechnique(0)->setLightingEnabled(false);
-  material_->setCullingMode(Ogre::CULL_NONE);
-  material_->setDepthBias(-1.f, 0.f);
-  material_->setDepthWriteEnabled(false);
   // 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
@@ -96,7 +65,6 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
   visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>(
       "" /* title */, visible, "" /* description */, submap_category,
       SLOT(ToggleVisibility()), this);
-  submap_node_->attachObject(manual_object_);
   scene_node_->setVisible(visible);
   connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
 }
@@ -107,14 +75,7 @@ DrawableSubmap::~DrawableSubmap() {
   if (QueryInProgress()) {
     rpc_request_future_.wait();
   }
-  Ogre::MaterialManager::getSingleton().remove(material_->getHandle());
-  if (!texture_.isNull()) {
-    Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
-    texture_.setNull();
-  }
-  display_context_->getSceneManager()->destroySceneNode(submap_node_);
   display_context_->getSceneManager()->destroySceneNode(scene_node_);
-  display_context_->getSceneManager()->destroyManualObject(manual_object_);
 }
 
 void DrawableSubmap::Update(
@@ -132,9 +93,7 @@ void DrawableSubmap::Update(
   pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
   scene_node_->setPosition(ToOgre(pose_.translation()));
   scene_node_->setOrientation(ToOgre(pose_.rotation()));
-  if (submap_texture_ != nullptr) {
+  display_context_->queueRender();
-    display_context_->queueRender();
-  }
   visibility_->setName(
       QString("%1.%2").arg(id_.submap_index).arg(metadata_version_));
   visibility_->setDescription(
@@ -187,84 +146,26 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) {
       std::abs(pose_.translation().z() - current_tracking_z);
   const double fade_distance =
       std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
-  const float alpha = static_cast<float>(
+  const float target_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_);
-  submap_node_->setPosition(ToOgre(submap_texture_->slice_pose.translation()));
-  submap_node_->setOrientation(ToOgre(submap_texture_->slice_pose.rotation()));
-  // 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.
-  std::vector<char> rgb;
-  for (size_t i = 0; i < submap_texture_->intensity.size(); ++i) {
-    rgb.push_back(submap_texture_->intensity[i]);
-    rgb.push_back(submap_texture_->alpha[i]);
-    rgb.push_back(0.);
-  }
-
-  manual_object_->clear();
-  const float metric_width =
-      submap_texture_->resolution * submap_texture_->width;
-  const float metric_height =
-      submap_texture_->resolution * submap_texture_->height;
-  manual_object_->begin(material_->getName(),
-                        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;
-  pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size()));
-
-  if (!texture_.isNull()) {
-    Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
-    texture_.setNull();
-  }
-  const std::string texture_name =
-      kSubmapTexturePrefix + GetSubmapIdentifier(id_);
-  texture_ = Ogre::TextureManager::getSingleton().loadRawData(
-      texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
-      pixel_stream, submap_texture_->width, submap_texture_->height,
-      Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
-
-  Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0);
-  pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
-  Ogre::TextureUnitState* const texture_unit =
-      pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0)
-                                          : pass->createTextureUnitState();
-
-  texture_unit->setTextureName(texture_->getName());
-  texture_unit->setTextureFiltering(Ogre::TFO_NONE);
-
-  display_context_->queueRender();
-}
-
-float DrawableSubmap::UpdateAlpha(const float target_alpha) {
   if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold ||
       target_alpha == 0.f || target_alpha == 1.f) {
     current_alpha_ = target_alpha;
   }
-  return current_alpha_;
+  ogre_submap_.SetAlpha(current_alpha_);
+  display_context_->queueRender();
+}
+
+void DrawableSubmap::UpdateSceneNode() {
+  ::cartographer::common::MutexLocker locker(&mutex_);
+  ogre_submap_.Update(*submap_texture_);
+  display_context_->queueRender();
 }
 
 void DrawableSubmap::ToggleVisibility() {
   scene_node_->setVisible(visibility_->getBool());
+  display_context_->queueRender();
 }
 
 }  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -22,19 +22,15 @@
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
-#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/mapping/id.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer_ros/submap.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
+#include "cartographer_rviz/ogre_submap.h"
 #include "ros/ros.h"
 #include "rviz/display_context.h"
 #include "rviz/frame_manager.h"
@@ -91,17 +87,12 @@ class DrawableSubmap : public QObject {
   void ToggleVisibility();
 
  private:
-  float UpdateAlpha(float target_alpha);
-
   const ::cartographer::mapping::SubmapId id_;
 
   ::cartographer::common::Mutex mutex_;
   ::rviz::DisplayContext* const display_context_;
   Ogre::SceneNode* const scene_node_;
-  Ogre::SceneNode* const submap_node_;
+  OgreSubmap ogre_submap_;
-  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_);

cartographer_rviz/cartographer_rviz/ogre_submap.cc

@@ -0,0 +1,145 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_rviz/ogre_submap.h"
+
+#include <string>
+#include <vector>
+
+#include "OgreGpuProgramParams.h"
+#include "OgreImage.h"
+#include "cartographer/common/port.h"
+
+namespace cartographer_rviz {
+
+namespace {
+
+constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
+constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap";
+constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
+constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
+
+std::string GetSubmapIdentifier(
+    const ::cartographer::mapping::SubmapId& submap_id) {
+  return std::to_string(submap_id.trajectory_id) + "-" +
+         std::to_string(submap_id.submap_index);
+}
+
+}  // namespace
+
+Ogre::Vector3 ToOgre(const Eigen::Vector3d& v) {
+  return Ogre::Vector3(v.x(), v.y(), v.z());
+}
+
+Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
+  return Ogre::Quaternion(q.w(), q.x(), q.y(), q.z());
+}
+
+OgreSubmap::OgreSubmap(const ::cartographer::mapping::SubmapId& id,
+                       Ogre::SceneManager* const scene_manager,
+                       Ogre::SceneNode* const scene_node)
+    : id_(id),
+      scene_manager_(scene_manager),
+      scene_node_(scene_node),
+      submap_node_(scene_node_->createChildSceneNode()),
+      manual_object_(scene_manager_->createManualObject(
+          kManualObjectPrefix + GetSubmapIdentifier(id))) {
+  material_ = Ogre::MaterialManager::getSingleton().getByName(
+      kSubmapSourceMaterialName);
+  material_ =
+      material_->clone(kSubmapMaterialPrefix + GetSubmapIdentifier(id_));
+  material_->setReceiveShadows(false);
+  material_->getTechnique(0)->setLightingEnabled(false);
+  material_->setCullingMode(Ogre::CULL_NONE);
+  material_->setDepthBias(-1.f, 0.f);
+  material_->setDepthWriteEnabled(false);
+  submap_node_->attachObject(manual_object_);
+}
+
+OgreSubmap::~OgreSubmap() {
+  Ogre::MaterialManager::getSingleton().remove(material_->getHandle());
+  if (!texture_.isNull()) {
+    Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
+    texture_.setNull();
+  }
+  scene_manager_->destroySceneNode(submap_node_);
+  scene_manager_->destroyManualObject(manual_object_);
+}
+
+void OgreSubmap::Update(
+    const ::cartographer_ros::SubmapTexture& submap_texture) {
+  submap_node_->setPosition(ToOgre(submap_texture.slice_pose.translation()));
+  submap_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation()));
+  // 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.
+  std::vector<char> rgb;
+  CHECK_EQ(submap_texture.intensity.size(), submap_texture.alpha.size());
+  for (size_t i = 0; i < submap_texture.intensity.size(); ++i) {
+    rgb.push_back(submap_texture.intensity[i]);
+    rgb.push_back(submap_texture.alpha[i]);
+    rgb.push_back(0);
+  }
+
+  manual_object_->clear();
+  const float metric_width = submap_texture.resolution * submap_texture.width;
+  const float metric_height = submap_texture.resolution * submap_texture.height;
+  manual_object_->begin(material_->getName(),
+                        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;
+  pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size()));
+
+  if (!texture_.isNull()) {
+    Ogre::TextureManager::getSingleton().remove(texture_->getHandle());
+    texture_.setNull();
+  }
+  const std::string texture_name =
+      kSubmapTexturePrefix + GetSubmapIdentifier(id_);
+  texture_ = Ogre::TextureManager::getSingleton().loadRawData(
+      texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+      pixel_stream, submap_texture.width, submap_texture.height,
+      Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
+
+  Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0);
+  pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
+  Ogre::TextureUnitState* const texture_unit =
+      pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0)
+                                          : pass->createTextureUnitState();
+
+  texture_unit->setTextureName(texture_->getName());
+  texture_unit->setTextureFiltering(Ogre::TFO_NONE);
+}
+
+void OgreSubmap::SetAlpha(const float alpha) {
+  const Ogre::GpuProgramParametersSharedPtr parameters =
+      material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
+  parameters->setNamedConstant("u_alpha", alpha);
+}
+
+}  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/ogre_submap.h

@@ -0,0 +1,70 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_RVIZ_SRC_OGRE_SUBMAP_H_
+#define CARTOGRAPHER_RVIZ_SRC_OGRE_SUBMAP_H_
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "OgreManualObject.h"
+#include "OgreMaterial.h"
+#include "OgreQuaternion.h"
+#include "OgreSceneManager.h"
+#include "OgreSceneNode.h"
+#include "OgreTexture.h"
+#include "OgreVector3.h"
+#include "cartographer/mapping/id.h"
+#include "cartographer_ros/submap.h"
+
+namespace cartographer_rviz {
+
+Ogre::Vector3 ToOgre(const Eigen::Vector3d& v);
+Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q);
+
+// A class containing the Ogre code to visualize submap data. Member functions
+// are expected to be called from the Ogre thread.
+class OgreSubmap {
+ public:
+  // Attaches a node visualizing the submap 'id' to the 'scene_node' which is
+  // expected to represent the submap frame.
+  OgreSubmap(const ::cartographer::mapping::SubmapId& id,
+             Ogre::SceneManager* const scene_manager,
+             Ogre::SceneNode* const scene_node);
+  ~OgreSubmap();
+
+  OgreSubmap(const OgreSubmap&) = delete;
+  OgreSubmap& operator=(const OgreSubmap&) = delete;
+
+  // Updates the texture and pose of the submap using new data from
+  // 'submap_texture'.
+  void Update(const ::cartographer_ros::SubmapTexture& submap_texture);
+
+  // Changes the opacity of the submap to 'alpha'.
+  void SetAlpha(float alpha);
+
+ private:
+  const ::cartographer::mapping::SubmapId id_;
+  Ogre::SceneManager* const scene_manager_;
+  Ogre::SceneNode* const scene_node_;
+  Ogre::SceneNode* const submap_node_;
+  Ogre::ManualObject* const manual_object_;
+  Ogre::TexturePtr texture_;
+  Ogre::MaterialPtr material_;
+};
+
+}  // namespace cartographer_rviz
+
+#endif  // CARTOGRAPHER_RVIZ_SRC_OGRE_SUBMAP_H_