From fed96d8ceef9bfffdd1c4dd0d074a6e377d24bbf Mon Sep 17 00:00:00 2001
From: Holger Rapp <HolgerRapp@gmx.net>
Date: Thu, 20 Jul 2017 15:10:07 +0200
Subject: [PATCH] Reuse code from cartographer_ros in cartographer_rviz. (#431)

Refactor a bit for readability.
---
 cartographer_rviz/CMakeLists.txt              |  1 +
 .../cartographer_rviz/drawable_submap.cc      | 25 +++++++++++--------
 .../cartographer_rviz/drawable_submap.h       |  6 ++---
 cartographer_rviz/package.xml                 |  1 +
 4 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/cartographer_rviz/CMakeLists.txt b/cartographer_rviz/CMakeLists.txt
index 3e2144d..074c6e3 100644
--- a/cartographer_rviz/CMakeLists.txt
+++ b/cartographer_rviz/CMakeLists.txt
@@ -17,6 +17,7 @@ cmake_minimum_required(VERSION 2.8.12)  # Ships with Ubuntu 14.04 (Trusty)
 project(cartographer_rviz)
 
 set(PACKAGE_DEPENDENCIES
+  cartographer_ros
   cartographer_ros_msgs
   eigen_conversions
   message_runtime
diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc
index fa7569d..9a3af1f 100644
--- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc
+++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc
@@ -27,6 +27,7 @@
 #include "OgreImage.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
+#include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "eigen_conversions/eigen_msg.h"
 #include "ros/ros.h"
@@ -53,6 +54,14 @@ std::string GetSubmapIdentifier(
          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,
@@ -111,10 +120,9 @@ void DrawableSubmap::Update(
     return;
   }
   ::cartographer::common::MutexLocker locker(&mutex_);
-  position_ = position;
-  orientation_ = orientation;
   submap_z_ = metadata.pose.position.z;
   metadata_version_ = metadata.submap_version;
+  pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
   if (texture_version_ != -1) {
     // We have to update the transform since we are already displaying a texture
     // for this submap.
@@ -185,7 +193,7 @@ void DrawableSubmap::UpdateSceneNode() {
   std::string compressed_cells(response_.cells.begin(), response_.cells.end());
   std::string cells;
   ::cartographer::common::FastGunzipString(compressed_cells, &cells);
-  tf::poseMsgToEigen(response_.slice_pose, slice_pose_);
+  slice_pose_ = ::cartographer_ros::ToRigid3d(response_.slice_pose);
   UpdateTransform();
   query_in_progress_ = false;
   // The call to Ogre's loadRawData below does not work with an RG texture,
@@ -245,14 +253,9 @@ void DrawableSubmap::UpdateSceneNode() {
 }
 
 void DrawableSubmap::UpdateTransform() {
-  const Eigen::Quaterniond quaternion(slice_pose_.rotation());
-  const Ogre::Quaternion slice_rotation(quaternion.w(), quaternion.x(),
-                                        quaternion.y(), quaternion.z());
-  const Ogre::Vector3 slice_translation(slice_pose_.translation().x(),
-                                        slice_pose_.translation().y(),
-                                        slice_pose_.translation().z());
-  scene_node_->setPosition(orientation_ * slice_translation + position_);
-  scene_node_->setOrientation(orientation_ * slice_rotation);
+  const ::cartographer::transform::Rigid3d pose = pose_ * slice_pose_;
+  scene_node_->setPosition(ToOgre(pose.translation()));
+  scene_node_->setOrientation(ToOgre(pose.rotation()));
 }
 
 float DrawableSubmap::UpdateAlpha(const float target_alpha) {
diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h
index a75de3f..1bb665a 100644
--- a/cartographer_rviz/cartographer_rviz/drawable_submap.h
+++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h
@@ -30,6 +30,7 @@
 #include "OgreVector3.h"
 #include "cartographer/common/mutex.h"
 #include "cartographer/mapping/id.h"
+#include "cartographer/transform/rigid_transform.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "ros/ros.h"
@@ -99,9 +100,8 @@ class DrawableSubmap : public QObject {
   Ogre::TexturePtr texture_;
   Ogre::MaterialPtr material_;
   double submap_z_ = 0. GUARDED_BY(mutex_);
-  Ogre::Vector3 position_ GUARDED_BY(mutex_);
-  Ogre::Quaternion orientation_ GUARDED_BY(mutex_);
-  Eigen::Affine3d slice_pose_ GUARDED_BY(mutex_);
+  ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
+  ::cartographer::transform::Rigid3d slice_pose_ GUARDED_BY(mutex_);
   std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
   bool query_in_progress_ = false GUARDED_BY(mutex_);
   int metadata_version_ = -1 GUARDED_BY(mutex_);
diff --git a/cartographer_rviz/package.xml b/cartographer_rviz/package.xml
index f7271e4..cc83cf8 100644
--- a/cartographer_rviz/package.xml
+++ b/cartographer_rviz/package.xml
@@ -35,6 +35,7 @@
   <build_depend>g++-static</build_depend>
 
   <depend>cartographer</depend>
+  <depend>cartographer_ros</depend>
   <depend>cartographer_ros_msgs</depend>
   <depend>eigen_conversions</depend>
   <depend>libqt5-core</depend>