Merge pull request #946 from bponsler/tf-load-frames

Fix for tf display not loading frame config
ros-visualization · Dec 10, 2015 · 39bd4be · 39bd4be
2 parents 428126c + 0baa9f3
commit 39bd4be
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 0 deletions.
diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp
@@ -206,13 +206,36 @@ TFDisplay::~TFDisplay()
 
 void TFDisplay::onInitialize()
 {
+  frame_config_enabled_state_.clear();
+
   root_node_ = scene_node_->createChildSceneNode();
 
   names_node_ = root_node_->createChildSceneNode();
   arrows_node_ = root_node_->createChildSceneNode();
   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
@@ -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_;