tf_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <boost/bind.hpp>
00031 
00032 #include <OgreSceneNode.h>
00033 #include <OgreSceneManager.h>
00034 
00035 #include <tf/transform_listener.h>
00036 
00037 #include "rviz/display_context.h"
00038 #include "rviz/frame_manager.h"
00039 #include "rviz/ogre_helpers/arrow.h"
00040 #include "rviz/ogre_helpers/axes.h"
00041 #include "rviz/ogre_helpers/movable_text.h"
00042 #include "rviz/properties/bool_property.h"
00043 #include "rviz/properties/float_property.h"
00044 #include "rviz/properties/quaternion_property.h"
00045 #include "rviz/properties/string_property.h"
00046 #include "rviz/properties/vector_property.h"
00047 #include "rviz/selection/forwards.h"
00048 #include "rviz/selection/selection_manager.h"
00049 
00050 #include "rviz/default_plugin/tf_display.h"
00051 
00052 namespace rviz
00053 {
00054 
00055 class FrameSelectionHandler: public SelectionHandler
00056 {
00057 public:
00058   FrameSelectionHandler( FrameInfo* frame, TFDisplay* display, DisplayContext* context );
00059   virtual ~FrameSelectionHandler() {}
00060 
00061   virtual void createProperties( const Picked& obj, Property* parent_property );
00062   virtual void destroyProperties( const Picked& obj, Property* parent_property );
00063 
00064   bool getEnabled();
00065   void setEnabled( bool enabled );
00066   void setParentName( std::string parent_name );
00067   void setPosition( const Ogre::Vector3& position );
00068   void setOrientation( const Ogre::Quaternion& orientation );
00069 
00070 private:
00071   FrameInfo* frame_;
00072   TFDisplay* display_;
00073   Property* category_property_;
00074   BoolProperty* enabled_property_;
00075   StringProperty* parent_property_;
00076   VectorProperty* position_property_;
00077   QuaternionProperty* orientation_property_;
00078 };
00079 
00080 FrameSelectionHandler::FrameSelectionHandler(FrameInfo* frame, TFDisplay* display, DisplayContext* context )
00081   : SelectionHandler( context )
00082   , frame_( frame )
00083   , display_( display )
00084   , category_property_( NULL )
00085   , enabled_property_( NULL )
00086   , parent_property_( NULL )
00087   , position_property_( NULL )
00088   , orientation_property_( NULL )
00089 {
00090 }
00091 
00092 void FrameSelectionHandler::createProperties( const Picked& obj, Property* parent_property )
00093 {
00094   category_property_ = new Property( "Frame " + QString::fromStdString( frame_->name_ ), QVariant(), "", parent_property );
00095 
00096   enabled_property_ = new BoolProperty( "Enabled", true, "", category_property_, SLOT( updateVisibilityFromSelection() ), frame_ );
00097 
00098   parent_property_ = new StringProperty( "Parent", "", "", category_property_ );
00099   parent_property_->setReadOnly( true );
00100 
00101   position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, "", category_property_ );
00102   position_property_->setReadOnly( true );
00103 
00104   orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, "", category_property_ );
00105   orientation_property_->setReadOnly( true );
00106 }
00107 
00108 void FrameSelectionHandler::destroyProperties( const Picked& obj, Property* parent_property )
00109 {
00110   delete category_property_; // This deletes its children as well.
00111   category_property_ = NULL;
00112   enabled_property_ = NULL;
00113   parent_property_ = NULL;
00114   position_property_ = NULL;
00115   orientation_property_ = NULL;
00116 }
00117 
00118 bool FrameSelectionHandler::getEnabled()
00119 {
00120   if( enabled_property_ )
00121   {
00122     return enabled_property_->getBool();
00123   }
00124   return false; // should never happen, but don't want to crash if it does.
00125 }
00126 
00127 void FrameSelectionHandler::setEnabled( bool enabled )
00128 {
00129   if( enabled_property_ )
00130   {
00131     enabled_property_->setBool( enabled );
00132   }
00133 }
00134 
00135 void FrameSelectionHandler::setParentName( std::string parent_name )
00136 {
00137   if( parent_property_ )
00138   {
00139     parent_property_->setStdString( parent_name );
00140   }
00141 }
00142 
00143 void FrameSelectionHandler::setPosition( const Ogre::Vector3& position )
00144 {
00145   if( position_property_ )
00146   {
00147     position_property_->setVector( position );
00148   }
00149 }
00150 
00151 void FrameSelectionHandler::setOrientation( const Ogre::Quaternion& orientation )
00152 {
00153   if( orientation_property_ )
00154   {
00155     orientation_property_->setQuaternion( orientation );
00156   }
00157 }
00158 
00159 typedef std::set<FrameInfo*> S_FrameInfo;
00160 
00161 TFDisplay::TFDisplay()
00162   : Display()
00163   , update_timer_( 0.0f )
00164   , changing_single_frame_enabled_state_( false )
00165 {
00166   show_names_property_ = new BoolProperty( "Show Names", true, "Whether or not names should be shown next to the frames.",
00167                                            this, SLOT( updateShowNames() ));
00168 
00169   show_axes_property_ = new BoolProperty( "Show Axes", true, "Whether or not the axes of each frame should be shown.",
00170                                           this, SLOT( updateShowAxes() ));
00171 
00172   show_arrows_property_ = new BoolProperty( "Show Arrows", true, "Whether or not arrows from child to parent should be shown.",
00173                                             this, SLOT( updateShowArrows() ));
00174 
00175   scale_property_ = new FloatProperty( "Marker Scale", 1, "Scaling factor for all names, axes and arrows.", this );
00176 
00177   update_rate_property_ = new FloatProperty( "Update Interval", 0,
00178                                              "The interval, in seconds, at which to update the frame transforms.  0 means to do so every update cycle.",
00179                                              this );
00180   update_rate_property_->setMin( 0 );
00181 
00182   frame_timeout_property_ = new FloatProperty( "Frame Timeout", 15,
00183                                                "The length of time, in seconds, before a frame that has not been updated is considered \"dead\"."
00184                                                "  For 1/3 of this time the frame will appear correct, for the second 1/3rd it will fade to gray,"
00185                                                " and then it will fade out completely.",
00186                                                this );
00187   frame_timeout_property_->setMin( 1 );
00188 
00189   frames_category_ = new Property( "Frames", QVariant(), "The list of all frames.", this );
00190 
00191   all_enabled_property_ = new BoolProperty( "All Enabled", true,
00192                                             "Whether all the frames should be enabled or not.",
00193                                             frames_category_, SLOT( allEnabledChanged() ), this );
00194 
00195   tree_category_ = new Property( "Tree", QVariant(), "A tree-view of the frames, showing the parent/child relationships.", this );
00196 }
00197 
00198 TFDisplay::~TFDisplay()
00199 {
00200   if ( initialized() )
00201   {
00202     root_node_->removeAndDestroyAllChildren();
00203     scene_manager_->destroySceneNode( root_node_->getName() );
00204   }
00205 }
00206 
00207 void TFDisplay::onInitialize()
00208 {
00209   frame_config_enabled_state_.clear();
00210 
00211   root_node_ = scene_node_->createChildSceneNode();
00212 
00213   names_node_ = root_node_->createChildSceneNode();
00214   arrows_node_ = root_node_->createChildSceneNode();
00215   axes_node_ = root_node_->createChildSceneNode();
00216 }
00217 
00218 void TFDisplay::load(const Config& config)
00219 {
00220   Display::load(config);
00221 
00222   // Load the enabled state for all frames specified in the config, and store
00223   // the values in a map so that the enabled state can be properly set once
00224   // the frame is created
00225   Config c = config.mapGetChild("Frames");
00226   for( Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance() )
00227   {
00228     QString key = iter.currentKey();
00229     if( key != "All Enabled" )
00230     {
00231       const Config& child = iter.currentChild();
00232       bool enabled = child.mapGetChild("Value").getValue().toBool();
00233 
00234       frame_config_enabled_state_[key.toStdString()] = enabled;
00235     }
00236   }
00237 }
00238 
00239 void TFDisplay::clear()
00240 {
00241   // Clear the tree.
00242   tree_category_->removeChildren();
00243 
00244   // Clear the frames category, except for the "All enabled" property, which is first.
00245   frames_category_->removeChildren( 1 );
00246 
00247   S_FrameInfo to_delete;
00248   M_FrameInfo::iterator frame_it = frames_.begin();
00249   M_FrameInfo::iterator frame_end = frames_.end();
00250   for ( ; frame_it != frame_end; ++frame_it )
00251   {
00252     to_delete.insert( frame_it->second );
00253   }
00254 
00255   S_FrameInfo::iterator delete_it = to_delete.begin();
00256   S_FrameInfo::iterator delete_end = to_delete.end();
00257   for ( ; delete_it != delete_end; ++delete_it )
00258   {
00259     deleteFrame( *delete_it, false );
00260   }
00261 
00262   frames_.clear();
00263 
00264   update_timer_ = 0.0f;
00265 
00266   clearStatuses();
00267 }
00268 
00269 void TFDisplay::onEnable()
00270 {
00271   root_node_->setVisible( true );
00272 
00273   names_node_->setVisible( show_names_property_->getBool() );
00274   arrows_node_->setVisible( show_arrows_property_->getBool() );
00275   axes_node_->setVisible( show_axes_property_->getBool() );
00276 }
00277 
00278 void TFDisplay::onDisable()
00279 {
00280   root_node_->setVisible( false );
00281   clear();
00282 }
00283 
00284 void TFDisplay::updateShowNames()
00285 {
00286   names_node_->setVisible( show_names_property_->getBool() );
00287 
00288   M_FrameInfo::iterator it = frames_.begin();
00289   M_FrameInfo::iterator end = frames_.end();
00290   for (; it != end; ++it)
00291   {
00292     FrameInfo* frame = it->second;
00293 
00294     frame->updateVisibilityFromFrame();
00295   }
00296 }
00297 
00298 void TFDisplay::updateShowAxes()
00299 {
00300   axes_node_->setVisible( show_axes_property_->getBool() );
00301 
00302   M_FrameInfo::iterator it = frames_.begin();
00303   M_FrameInfo::iterator end = frames_.end();
00304   for (; it != end; ++it)
00305   {
00306     FrameInfo* frame = it->second;
00307 
00308     frame->updateVisibilityFromFrame();
00309   }
00310 }
00311 
00312 void TFDisplay::updateShowArrows()
00313 {
00314   arrows_node_->setVisible( show_arrows_property_->getBool() );
00315 
00316   M_FrameInfo::iterator it = frames_.begin();
00317   M_FrameInfo::iterator end = frames_.end();
00318   for (; it != end; ++it)
00319   {
00320     FrameInfo* frame = it->second;
00321 
00322     frame->updateVisibilityFromFrame();
00323   }
00324 }
00325 
00326 void TFDisplay::allEnabledChanged()
00327 {
00328   if( changing_single_frame_enabled_state_ )
00329   {
00330     return;
00331   }
00332   bool enabled = all_enabled_property_->getBool();
00333 
00334   M_FrameInfo::iterator it = frames_.begin();
00335   M_FrameInfo::iterator end = frames_.end();
00336   for (; it != end; ++it)
00337   {
00338     FrameInfo* frame = it->second;
00339 
00340     frame->enabled_property_->setBool( enabled );
00341   }
00342 }
00343 
00344 void TFDisplay::update(float wall_dt, float ros_dt)
00345 {
00346   update_timer_ += wall_dt;
00347   float update_rate = update_rate_property_->getFloat();
00348   if( update_rate < 0.0001f || update_timer_ > update_rate )
00349   {
00350     updateFrames();
00351 
00352     update_timer_ = 0.0f;
00353   }
00354 }
00355 
00356 FrameInfo* TFDisplay::getFrameInfo( const std::string& frame )
00357 {
00358   M_FrameInfo::iterator it = frames_.find( frame );
00359   if ( it == frames_.end() )
00360   {
00361     return NULL;
00362   }
00363 
00364   return it->second;
00365 }
00366 
00367 void TFDisplay::updateFrames()
00368 {
00369   typedef std::vector<std::string> V_string;
00370   V_string frames;
00371   context_->getTFClient()->getFrameStrings( frames );
00372   std::sort(frames.begin(), frames.end());
00373 
00374   S_FrameInfo current_frames;
00375 
00376   {
00377     V_string::iterator it = frames.begin();
00378     V_string::iterator end = frames.end();
00379     for ( ; it != end; ++it )
00380     {
00381       const std::string& frame = *it;
00382 
00383       if ( frame.empty() )
00384       {
00385         continue;
00386       }
00387 
00388       FrameInfo* info = getFrameInfo( frame );
00389       if (!info)
00390       {
00391         info = createFrame(frame);
00392       }
00393       else
00394       {
00395         updateFrame(info);
00396       }
00397 
00398       current_frames.insert( info );
00399     }
00400   }
00401 
00402   {
00403     S_FrameInfo to_delete;
00404     M_FrameInfo::iterator frame_it = frames_.begin();
00405     M_FrameInfo::iterator frame_end = frames_.end();
00406     for ( ; frame_it != frame_end; ++frame_it )
00407     {
00408       if ( current_frames.find( frame_it->second ) == current_frames.end() )
00409       {
00410         to_delete.insert( frame_it->second );
00411       }
00412     }
00413 
00414     S_FrameInfo::iterator delete_it = to_delete.begin();
00415     S_FrameInfo::iterator delete_end = to_delete.end();
00416     for ( ; delete_it != delete_end; ++delete_it )
00417     {
00418       deleteFrame( *delete_it, true );
00419     }
00420   }
00421 
00422   context_->queueRender();
00423 }
00424 
00425 static const Ogre::ColourValue ARROW_HEAD_COLOR(1.0f, 0.1f, 0.6f, 1.0f);
00426 static const Ogre::ColourValue ARROW_SHAFT_COLOR(0.8f, 0.8f, 0.3f, 1.0f);
00427 
00428 FrameInfo* TFDisplay::createFrame(const std::string& frame)
00429 {
00430   FrameInfo* info = new FrameInfo( this );
00431   frames_.insert( std::make_pair( frame, info ) );
00432 
00433   info->name_ = frame;
00434   info->last_update_ = ros::Time::now();
00435   info->axes_ = new Axes( scene_manager_, axes_node_, 0.2, 0.02 );
00436   info->axes_->getSceneNode()->setVisible( show_axes_property_->getBool() );
00437   info->selection_handler_.reset( new FrameSelectionHandler( info, this, context_ ));
00438   info->selection_handler_->addTrackedObjects( info->axes_->getSceneNode() );
00439 
00440   info->name_text_ = new MovableText( frame, "Liberation Sans", 0.1 );
00441   info->name_text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_BELOW);
00442   info->name_node_ = names_node_->createChildSceneNode();
00443   info->name_node_->attachObject( info->name_text_ );
00444   info->name_node_->setVisible( show_names_property_->getBool() );
00445 
00446   info->parent_arrow_ = new Arrow( scene_manager_, arrows_node_, 1.0f, 0.01, 1.0f, 0.08 );
00447   info->parent_arrow_->getSceneNode()->setVisible( false );
00448   info->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR);
00449   info->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);
00450 
00451   info->enabled_property_ = new BoolProperty( QString::fromStdString( info->name_ ), true, "Enable or disable this individual frame.",
00452                                               frames_category_, SLOT( updateVisibilityFromFrame() ), info );
00453 
00454   info->parent_property_ = new StringProperty( "Parent", "", "Parent of this frame.  (Not editable)",
00455                                                info->enabled_property_ );
00456   info->parent_property_->setReadOnly( true );
00457 
00458   info->position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
00459                                                  "Position of this frame, in the current Fixed Frame.  (Not editable)",
00460                                                  info->enabled_property_ );
00461   info->position_property_->setReadOnly( true );
00462 
00463   info->orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
00464                                                         "Orientation of this frame, in the current Fixed Frame.  (Not editable)",
00465                                                         info->enabled_property_ );
00466   info->orientation_property_->setReadOnly( true );
00467 
00468   info->rel_position_property_ = new VectorProperty( "Relative Position", Ogre::Vector3::ZERO,
00469                                                  "Position of this frame, relative to it's parent frame.  (Not editable)",
00470                                                  info->enabled_property_ );
00471   info->rel_position_property_->setReadOnly( true );
00472 
00473   info->rel_orientation_property_ = new QuaternionProperty( "Relative Orientation", Ogre::Quaternion::IDENTITY,
00474                                                         "Orientation of this frame, relative to it's parent frame.  (Not editable)",
00475                                                         info->enabled_property_ );
00476   info->rel_orientation_property_->setReadOnly( true );
00477 
00478   // If the current frame was specified as disabled in the config file
00479   // then its enabled state must be updated accordingly
00480   if( frame_config_enabled_state_.count(frame) > 0 && !frame_config_enabled_state_[frame] )
00481   {
00482       info->enabled_property_->setBool(false);
00483   }
00484 
00485   updateFrame( info );
00486 
00487   return info;
00488 }
00489 
00490 Ogre::ColourValue lerpColor(const Ogre::ColourValue& start, const Ogre::ColourValue& end, float t)
00491 {
00492   return start * t + end * (1 - t);
00493 }
00494 
00495 void TFDisplay::updateFrame( FrameInfo* frame )
00496 {
00497   tf::TransformListener* tf = context_->getTFClient();
00498 
00499   // Check last received time so we can grey out/fade out frames that have stopped being published
00500   ros::Time latest_time;
00501   tf->getLatestCommonTime( fixed_frame_.toStdString(), frame->name_, latest_time, 0 );
00502 
00503   if(( latest_time != frame->last_time_to_fixed_ ) ||
00504      ( latest_time == ros::Time() ))
00505   {
00506     frame->last_update_ = ros::Time::now();
00507     frame->last_time_to_fixed_ = latest_time;
00508   }
00509 
00510   // Fade from color -> grey, then grey -> fully transparent
00511   ros::Duration age = ros::Time::now() - frame->last_update_;
00512   float frame_timeout = frame_timeout_property_->getFloat();
00513   float one_third_timeout = frame_timeout * 0.3333333f;
00514   if( age > ros::Duration( frame_timeout ))
00515   {
00516     frame->parent_arrow_->getSceneNode()->setVisible(false);
00517     frame->axes_->getSceneNode()->setVisible(false);
00518     frame->name_node_->setVisible(false);
00519     return;
00520   }
00521   else if (age > ros::Duration(one_third_timeout))
00522   {
00523     Ogre::ColourValue grey(0.7, 0.7, 0.7, 1.0);
00524 
00525     if (age > ros::Duration(one_third_timeout * 2))
00526     {
00527       float a = std::max(0.0, (frame_timeout - age.toSec())/one_third_timeout);
00528       Ogre::ColourValue c = Ogre::ColourValue(grey.r, grey.g, grey.b, a);
00529 
00530       frame->axes_->setXColor(c);
00531       frame->axes_->setYColor(c);
00532       frame->axes_->setZColor(c);
00533       frame->name_text_->setColor(c);
00534       frame->parent_arrow_->setColor(c.r, c.g, c.b, c.a);
00535     }
00536     else
00537     {
00538       float t = std::max(0.0, (one_third_timeout * 2 - age.toSec())/one_third_timeout);
00539       frame->axes_->setXColor(lerpColor(frame->axes_->getDefaultXColor(), grey, t));
00540       frame->axes_->setYColor(lerpColor(frame->axes_->getDefaultYColor(), grey, t));
00541       frame->axes_->setZColor(lerpColor(frame->axes_->getDefaultZColor(), grey, t));
00542       frame->name_text_->setColor(lerpColor(Ogre::ColourValue::White, grey, t));
00543       frame->parent_arrow_->setShaftColor(lerpColor(ARROW_SHAFT_COLOR, grey, t));
00544       frame->parent_arrow_->setHeadColor(lerpColor(ARROW_HEAD_COLOR, grey, t));
00545     }
00546   }
00547   else
00548   {
00549     frame->axes_->setToDefaultColors();
00550     frame->name_text_->setColor(Ogre::ColourValue::White);
00551     frame->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR);
00552     frame->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);
00553   }
00554 
00555   setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK");
00556 
00557   Ogre::Vector3 position;
00558   Ogre::Quaternion orientation;
00559   if( !context_->getFrameManager()->getTransform( frame->name_, ros::Time(), position, orientation ))
00560   {
00561     std::stringstream ss;
00562     ss << "No transform from [" << frame->name_ << "] to frame [" << fixed_frame_.toStdString() << "]";
00563     setStatusStd(StatusProperty::Warn, frame->name_, ss.str());
00564     ROS_DEBUG( "Error transforming frame '%s' to frame '%s'", frame->name_.c_str(), qPrintable( fixed_frame_ ));
00565     frame->name_node_->setVisible( false );
00566     frame->axes_->getSceneNode()->setVisible( false );
00567     frame->parent_arrow_->getSceneNode()->setVisible( false );
00568     return;
00569   }
00570 
00571   frame->selection_handler_->setPosition( position );
00572   frame->selection_handler_->setOrientation( orientation );
00573 
00574   bool frame_enabled = frame->enabled_property_->getBool();
00575 
00576   frame->axes_->setPosition( position );
00577   frame->axes_->setOrientation( orientation );
00578   frame->axes_->getSceneNode()->setVisible( show_axes_property_->getBool() && frame_enabled);
00579   float scale = scale_property_->getFloat();
00580   frame->axes_->setScale( Ogre::Vector3( scale, scale, scale ));
00581 
00582   frame->name_node_->setPosition( position );
00583   frame->name_node_->setVisible( show_names_property_->getBool() && frame_enabled );
00584   frame->name_node_->setScale( scale, scale, scale );
00585 
00586   frame->position_property_->setVector( position );
00587   frame->orientation_property_->setQuaternion( orientation );
00588 
00589   std::string old_parent = frame->parent_;
00590   frame->parent_.clear();
00591   bool has_parent = tf->getParent( frame->name_, ros::Time(), frame->parent_ );
00592   if( has_parent )
00593   {
00594     // If this frame has no tree property or the parent has changed,
00595     if( !frame->tree_property_ || old_parent != frame->parent_ )
00596     {
00597       // Look up the new parent.
00598       M_FrameInfo::iterator parent_it = frames_.find( frame->parent_ );
00599       if( parent_it != frames_.end() )
00600       {
00601         FrameInfo* parent = parent_it->second;
00602 
00603         // If the parent has a tree property, make a new tree property for this frame.
00604         if( parent->tree_property_ )
00605         {
00606           if(!frame->tree_property_) {
00607             frame->tree_property_ = new Property( QString::fromStdString( frame->name_ ), QVariant(), "", parent->tree_property_ );
00608           } else {
00609             frame->tree_property_->setParent(parent->tree_property_);
00610             frame->tree_property_->setName(QString::fromStdString( frame->name_ ));
00611             frame->tree_property_->setValue(QVariant());
00612             frame->tree_property_->setDescription("");
00613           }
00614         }
00615       }
00616     }
00617 
00618     tf::StampedTransform transform;
00619     try {
00620       context_->getFrameManager()->getTFClientPtr()->lookupTransform(frame->parent_,frame->name_,ros::Time(0),transform);
00621     }
00622     catch(tf::TransformException& e)
00623     {
00624       ROS_DEBUG( "Error transforming frame '%s' (parent of '%s') to frame '%s'",
00625                  frame->parent_.c_str(), frame->name_.c_str(), qPrintable( fixed_frame_ ));
00626     }
00627 
00628     // get the position/orientation relative to the parent frame
00629     Ogre::Vector3 relative_position( transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z() );
00630     Ogre::Quaternion relative_orientation( transform.getRotation().w(), transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z() );
00631     frame->rel_position_property_->setVector( relative_position );
00632     frame->rel_orientation_property_->setQuaternion( relative_orientation );
00633 
00634     if( show_arrows_property_->getBool() )
00635     {
00636       Ogre::Vector3 parent_position;
00637       Ogre::Quaternion parent_orientation;
00638       if (!context_->getFrameManager()->getTransform(frame->parent_, ros::Time(), parent_position, parent_orientation))
00639       {
00640         ROS_DEBUG( "Error transforming frame '%s' (parent of '%s') to frame '%s'",
00641                    frame->parent_.c_str(), frame->name_.c_str(), qPrintable( fixed_frame_ ));
00642       }
00643 
00644       Ogre::Vector3 direction = parent_position - position;
00645       float distance = direction.length();
00646       direction.normalise();
00647 
00648       Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
00649 
00650       frame->distance_to_parent_ = distance;
00651       float head_length = ( distance < 0.1*scale ) ? (0.1*scale*distance) : 0.1*scale;
00652       float shaft_length = distance - head_length;
00653       // aleeper: This was changed from 0.02 and 0.08 to 0.01 and 0.04 to match proper radius handling in arrow.cpp
00654       frame->parent_arrow_->set( shaft_length, 0.01*scale, head_length, 0.04*scale );
00655 
00656       if ( distance > 0.001f )
00657       {
00658         frame->parent_arrow_->getSceneNode()->setVisible( show_arrows_property_->getBool() && frame_enabled );
00659       }
00660       else
00661       {
00662         frame->parent_arrow_->getSceneNode()->setVisible( false );
00663       }
00664 
00665       frame->parent_arrow_->setPosition( position );
00666       frame->parent_arrow_->setOrientation( orient );
00667     }
00668     else
00669     {
00670       frame->parent_arrow_->getSceneNode()->setVisible( false );
00671     }
00672   }
00673   else
00674   {
00675     if ( !frame->tree_property_ || old_parent != frame->parent_ )
00676     {
00677       if(!frame->tree_property_) {
00678         frame->tree_property_ = new Property( QString::fromStdString( frame->name_ ), QVariant(), "", tree_category_ );
00679       } else {
00680         frame->tree_property_->setName(QString::fromStdString( frame->name_ ));
00681         frame->tree_property_->setValue(QVariant());
00682         frame->tree_property_->setDescription("");
00683         frame->tree_property_->setParent(tree_category_);
00684       }
00685     }
00686 
00687     frame->parent_arrow_->getSceneNode()->setVisible( false );
00688   }
00689 
00690   frame->parent_property_->setStdString( frame->parent_ );
00691   frame->selection_handler_->setParentName( frame->parent_ );
00692 }
00693 
00694 void TFDisplay::deleteFrame( FrameInfo* frame, bool delete_properties )
00695 {
00696   M_FrameInfo::iterator it = frames_.find( frame->name_ );
00697   ROS_ASSERT( it != frames_.end() );
00698 
00699   frames_.erase( it );
00700 
00701   delete frame->axes_;
00702   context_->getSelectionManager()->removeObject( frame->axes_coll_ );
00703   delete frame->parent_arrow_;
00704   delete frame->name_text_;
00705   scene_manager_->destroySceneNode( frame->name_node_->getName() );
00706   if( delete_properties )
00707   {
00708     delete frame->enabled_property_;
00709     delete frame->tree_property_;
00710   }
00711   delete frame;
00712 }
00713 
00714 void TFDisplay::fixedFrameChanged()
00715 {
00716   update_timer_ = update_rate_property_->getFloat();
00717 }
00718 
00719 void TFDisplay::reset()
00720 {
00721   Display::reset();
00722   clear();
00723 }
00724 
00726 // FrameInfo
00727 
00728 FrameInfo::FrameInfo( TFDisplay* display )
00729   : display_( display )
00730   , axes_( NULL )
00731   , axes_coll_( 0 )
00732   , parent_arrow_( NULL )
00733   , name_text_( NULL )
00734   , distance_to_parent_( 0.0f )
00735   , arrow_orientation_(Ogre::Quaternion::IDENTITY)
00736   , tree_property_( NULL )
00737 {}
00738 
00739 void FrameInfo::updateVisibilityFromFrame()
00740 {
00741   bool enabled = enabled_property_->getBool();
00742   selection_handler_->setEnabled( enabled );
00743   setEnabled( enabled );
00744 }
00745 
00746 void FrameInfo::updateVisibilityFromSelection()
00747 {
00748   bool enabled = selection_handler_->getEnabled();
00749   enabled_property_->setBool( enabled );
00750   setEnabled( enabled );
00751 }
00752 
00753 void FrameInfo::setEnabled( bool enabled )
00754 {
00755   if( name_node_ )
00756   {
00757     name_node_->setVisible( display_->show_names_property_->getBool() && enabled );
00758   }
00759 
00760   if( axes_ )
00761   {
00762     axes_->getSceneNode()->setVisible( display_->show_axes_property_->getBool() && enabled );
00763   }
00764 
00765   if( parent_arrow_ )
00766   {
00767     if( distance_to_parent_ > 0.001f )
00768     {
00769       parent_arrow_->getSceneNode()->setVisible( display_->show_arrows_property_->getBool() && enabled );
00770     }
00771     else
00772     {
00773       parent_arrow_->getSceneNode()->setVisible( false );
00774     }
00775   }
00776 
00777   if( display_->all_enabled_property_->getBool() && !enabled)
00778   {
00779     display_->changing_single_frame_enabled_state_ = true;
00780     display_->all_enabled_property_->setBool( false );
00781     display_->changing_single_frame_enabled_state_ = false;
00782   }
00783 
00784   // Update the configuration that stores the enabled state of all frames
00785   display_->frame_config_enabled_state_[this->name_] = enabled;
00786 
00787   display_->context_->queueRender();
00788 }
00789 
00790 } // namespace rviz
00791 
00792 #include <pluginlib/class_list_macros.h>
00793 PLUGINLIB_EXPORT_CLASS( rviz::TFDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31