From 0baa9f351cb73f880141ecfabd61aaa224de7458 Mon Sep 17 00:00:00 2001
From: bponsler <ponsler@gmail.com>
Date: Thu, 3 Dec 2015 10:58:53 -0800
Subject: [PATCH] Fixing issue with the tf display where the enabled status was
 not properly read from the config file. Updated the display to read the
 value, store the values in a map, and use the map to update the
 enabled/disabled state of the frame once the frame is added. This properly
 handles frames which are not published at the time the tf display is created.

---
 src/rviz/default_plugin/tf_display.cpp | 31 ++++++++++++++++++++++++++
 src/rviz/default_plugin/tf_display.h   |  4 ++++
 2 files changed, 35 insertions(+)

diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp
index 89a39fc38d..f7369ea0e0 100644
--- a/src/rviz/default_plugin/tf_display.cpp
+++ b/src/rviz/default_plugin/tf_display.cpp
@@ -206,6 +206,8 @@ TFDisplay::~TFDisplay()
 
 void TFDisplay::onInitialize()
 {
+  frame_config_enabled_state_.clear();
+
   root_node_ = scene_node_->createChildSceneNode();
 
   names_node_ = root_node_->createChildSceneNode();
@@ -213,6 +215,27 @@ void TFDisplay::onInitialize()
   axes_node_ = root_node_->createChildSceneNode();
 }
 
+void TFDisplay::load(const Config& config)
+{
+  Display::load(config);
+
+  // Load the enabled state for all frames specified in the config, and store
+  // the values in a map so that the enabled state can be properly set once
+  // the frame is created
+  Config c = config.mapGetChild("Frames");
+  for( Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance() )
+  {
+    QString key = iter.currentKey();
+    if( key != "All Enabled" )
+    {
+      const Config& child = iter.currentChild();
+      bool enabled = child.mapGetChild("Value").getValue().toBool();
+
+      frame_config_enabled_state_[key.toStdString()] = enabled;
+    }
+  }
+}
+
 void TFDisplay::clear()
 {
   // Clear the tree.
@@ -237,6 +260,7 @@ void TFDisplay::clear()
   }
 
   frames_.clear();
+  frame_config_enabled_state_.clear();
 
   update_timer_ = 0.0f;
 
@@ -452,6 +476,13 @@ FrameInfo* TFDisplay::createFrame(const std::string& frame)
                                                         info->enabled_property_ );
   info->rel_orientation_property_->setReadOnly( true );
 
+  // If the current frame was specified as disabled in the config file
+  // then its enabled state must be updated accordingly
+  if( frame_config_enabled_state_.count(frame) > 0 && !frame_config_enabled_state_[frame] )
+  {
+      info->enabled_property_->setBool(false);
+  }
+
   updateFrame( info );
 
   return info;
diff --git a/src/rviz/default_plugin/tf_display.h b/src/rviz/default_plugin/tf_display.h
index 1260c5bb10..d6265fe09d 100644
--- a/src/rviz/default_plugin/tf_display.h
+++ b/src/rviz/default_plugin/tf_display.h
@@ -73,6 +73,7 @@ Q_OBJECT
 protected:
   // Overrides from Display
   virtual void onInitialize();
+  virtual void load(const Config& config);
   virtual void fixedFrameChanged();
   virtual void reset();
 
@@ -104,6 +105,9 @@ private Q_SLOTS:
   typedef std::map<std::string, FrameInfo*> M_FrameInfo;
   M_FrameInfo frames_;
 
+  typedef std::map<std::string, bool> M_EnabledState;
+  M_EnabledState frame_config_enabled_state_;
+
   float update_timer_;
 
   BoolProperty* show_names_property_;