From f61e513bd6479895041a9f2918505250b5bb1eb9 Mon Sep 17 00:00:00 2001
From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com>
Date: Mon, 22 Jan 2018 16:43:49 +0100
Subject: [PATCH] Make fade out distance configurable (#674)

---
 .../cartographer_rviz/drawable_submap.cc           | 14 ++++++--------
 .../cartographer_rviz/drawable_submap.h            |  6 ++++--
 .../cartographer_rviz/submaps_display.cc           |  8 +++++++-
 .../cartographer_rviz/submaps_display.h            |  3 +++
 4 files changed, 20 insertions(+), 11 deletions(-)

diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc
index 1cb5cc8..afe9acf 100644
--- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc
+++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc
@@ -35,11 +35,6 @@ namespace cartographer_rviz {
 namespace {
 
 constexpr std::chrono::milliseconds kMinQueryDelayInMs(250);
-
-// Distance before which the submap will be shown at full opacity, and distance
-// over which the submap will then fade out.
-constexpr double kFadeOutStartDistanceInMeters = 1.;
-constexpr double kFadeOutDistanceInMeters = 2.;
 constexpr float kAlphaUpdateThreshold = 0.2f;
 
 const Ogre::ColourValue kSubmapIdColor(Ogre::ColourValue::Red);
@@ -154,13 +149,16 @@ bool DrawableSubmap::QueryInProgress() {
   return query_in_progress_;
 }
 
-void DrawableSubmap::SetAlpha(const double current_tracking_z) {
+void DrawableSubmap::SetAlpha(const double current_tracking_z,
+                              const float fade_out_start_distance_in_meters) {
+  const float fade_out_distance_in_meters =
+      2.f * fade_out_start_distance_in_meters;
   const double distance_z =
       std::abs(pose_.translation().z() - current_tracking_z);
   const double fade_distance =
-      std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
+      std::max(distance_z - fade_out_start_distance_in_meters, 0.);
   const float target_alpha = static_cast<float>(
-      std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters));
+      std::max(0., 1. - fade_distance / fade_out_distance_in_meters));
 
   if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold ||
       target_alpha == 0.f || target_alpha == 1.f) {
diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h
index 60a6421..ea3b154 100644
--- a/cartographer_rviz/cartographer_rviz/drawable_submap.h
+++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h
@@ -68,8 +68,10 @@ class DrawableSubmap : public QObject {
   bool QueryInProgress();
 
   // Sets the alpha of the submap taking into account its slice height and the
-  // 'current_tracking_z'.
-  void SetAlpha(double current_tracking_z);
+  // 'current_tracking_z'. 'fade_out_start_distance_in_meters' defines the
+  // distance in z direction in meters, before which the submap will be shown
+  // at full opacity.
+  void SetAlpha(double current_tracking_z, float fade_out_distance_in_meters);
 
   // Sets the visibility of a slice. It will be drawn if the parent submap
   // is also visible.
diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc
index 56f3101..d88d245 100644
--- a/cartographer_rviz/cartographer_rviz/submaps_display.cc
+++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc
@@ -66,6 +66,11 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
       "All", true,
       "Whether submaps from all trajectories should be displayed or not.",
       trajectories_category_, SLOT(AllEnabledToggled()), this);
+  fade_out_start_distance_in_meters_ =
+      new ::rviz::FloatProperty("Fade-out distance", 1.f,
+                                "Distance in meters in z-direction beyond "
+                                "which submaps will start to fade out.",
+                                this);
   const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
   Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
       package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
@@ -208,7 +213,8 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
     for (auto& trajectory : trajectories_) {
       for (auto& submap_entry : trajectory->submaps) {
         submap_entry.second->SetAlpha(
-            transform_stamped.transform.translation.z);
+            transform_stamped.transform.translation.z,
+            fade_out_start_distance_in_meters_->getFloat());
       }
     }
   } catch (const tf2::TransformException& ex) {
diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h
index 779725e..9b68b39 100644
--- a/cartographer_rviz/cartographer_rviz/submaps_display.h
+++ b/cartographer_rviz/cartographer_rviz/submaps_display.h
@@ -27,6 +27,8 @@
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_rviz/drawable_submap.h"
 #include "rviz/message_filter_display.h"
+#include "rviz/properties/bool_property.h"
+#include "rviz/properties/float_property.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
@@ -93,6 +95,7 @@ class SubmapsDisplay
   ::rviz::BoolProperty* slice_low_resolution_enabled_;
   ::rviz::Property* trajectories_category_;
   ::rviz::BoolProperty* visibility_all_enabled_;
+  ::rviz::FloatProperty* fade_out_start_distance_in_meters_;
 };
 
 }  // namespace cartographer_rviz