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   root_node_ = scene_node_->createChildSceneNode();
00210 
00211   names_node_ = root_node_->createChildSceneNode();
00212   arrows_node_ = root_node_->createChildSceneNode();
00213   axes_node_ = root_node_->createChildSceneNode();
00214 }
00215 
00216 void TFDisplay::clear()
00217 {
00218   // Clear the tree.
00219   tree_category_->removeChildren();
00220 
00221   // Clear the frames category, except for the "All enabled" property, which is first.
00222   frames_category_->removeChildren( 1 );
00223 
00224   S_FrameInfo to_delete;
00225   M_FrameInfo::iterator frame_it = frames_.begin();
00226   M_FrameInfo::iterator frame_end = frames_.end();
00227   for ( ; frame_it != frame_end; ++frame_it )
00228   {
00229     to_delete.insert( frame_it->second );
00230   }
00231 
00232   S_FrameInfo::iterator delete_it = to_delete.begin();
00233   S_FrameInfo::iterator delete_end = to_delete.end();
00234   for ( ; delete_it != delete_end; ++delete_it )
00235   {
00236     deleteFrame( *delete_it, false );
00237   }
00238 
00239   frames_.clear();
00240 
00241   update_timer_ = 0.0f;
00242 
00243   clearStatuses();
00244 }
00245 
00246 void TFDisplay::onEnable()
00247 {
00248   root_node_->setVisible( true );
00249 
00250   names_node_->setVisible( show_names_property_->getBool() );
00251   arrows_node_->setVisible( show_arrows_property_->getBool() );
00252   axes_node_->setVisible( show_axes_property_->getBool() );
00253 }
00254 
00255 void TFDisplay::onDisable()
00256 {
00257   root_node_->setVisible( false );
00258   clear();
00259 }
00260 
00261 void TFDisplay::updateShowNames()
00262 {
00263   names_node_->setVisible( show_names_property_->getBool() );
00264 
00265   M_FrameInfo::iterator it = frames_.begin();
00266   M_FrameInfo::iterator end = frames_.end();
00267   for (; it != end; ++it)
00268   {
00269     FrameInfo* frame = it->second;
00270 
00271     frame->updateVisibilityFromFrame();
00272   }
00273 }
00274 
00275 void TFDisplay::updateShowAxes()
00276 {
00277   axes_node_->setVisible( show_axes_property_->getBool() );
00278 
00279   M_FrameInfo::iterator it = frames_.begin();
00280   M_FrameInfo::iterator end = frames_.end();
00281   for (; it != end; ++it)
00282   {
00283     FrameInfo* frame = it->second;
00284 
00285     frame->updateVisibilityFromFrame();
00286   }
00287 }
00288 
00289 void TFDisplay::updateShowArrows()
00290 {
00291   arrows_node_->setVisible( show_arrows_property_->getBool() );
00292 
00293   M_FrameInfo::iterator it = frames_.begin();
00294   M_FrameInfo::iterator end = frames_.end();
00295   for (; it != end; ++it)
00296   {
00297     FrameInfo* frame = it->second;
00298 
00299     frame->updateVisibilityFromFrame();
00300   }
00301 }
00302 
00303 void TFDisplay::allEnabledChanged()
00304 {
00305   if( changing_single_frame_enabled_state_ )
00306   {
00307     return;
00308   }
00309   bool enabled = all_enabled_property_->getBool();
00310 
00311   M_FrameInfo::iterator it = frames_.begin();
00312   M_FrameInfo::iterator end = frames_.end();
00313   for (; it != end; ++it)
00314   {
00315     FrameInfo* frame = it->second;
00316 
00317     frame->enabled_property_->setBool( enabled );
00318   }
00319 }
00320 
00321 void TFDisplay::update(float wall_dt, float ros_dt)
00322 {
00323   update_timer_ += wall_dt;
00324   float update_rate = update_rate_property_->getFloat();
00325   if( update_rate < 0.0001f || update_timer_ > update_rate )
00326   {
00327     updateFrames();
00328 
00329     update_timer_ = 0.0f;
00330   }
00331 }
00332 
00333 FrameInfo* TFDisplay::getFrameInfo( const std::string& frame )
00334 {
00335   M_FrameInfo::iterator it = frames_.find( frame );
00336   if ( it == frames_.end() )
00337   {
00338     return NULL;
00339   }
00340 
00341   return it->second;
00342 }
00343 
00344 void TFDisplay::updateFrames()
00345 {
00346   typedef std::vector<std::string> V_string;
00347   V_string frames;
00348   context_->getTFClient()->getFrameStrings( frames );
00349   std::sort(frames.begin(), frames.end());
00350 
00351   S_FrameInfo current_frames;
00352 
00353   {
00354     V_string::iterator it = frames.begin();
00355     V_string::iterator end = frames.end();
00356     for ( ; it != end; ++it )
00357     {
00358       const std::string& frame = *it;
00359 
00360       if ( frame.empty() )
00361       {
00362         continue;
00363       }
00364 
00365       FrameInfo* info = getFrameInfo( frame );
00366       if (!info)
00367       {
00368         info = createFrame(frame);
00369       }
00370       else
00371       {
00372         updateFrame(info);
00373       }
00374 
00375       current_frames.insert( info );
00376     }
00377   }
00378 
00379   {
00380     S_FrameInfo to_delete;
00381     M_FrameInfo::iterator frame_it = frames_.begin();
00382     M_FrameInfo::iterator frame_end = frames_.end();
00383     for ( ; frame_it != frame_end; ++frame_it )
00384     {
00385       if ( current_frames.find( frame_it->second ) == current_frames.end() )
00386       {
00387         to_delete.insert( frame_it->second );
00388       }
00389     }
00390 
00391     S_FrameInfo::iterator delete_it = to_delete.begin();
00392     S_FrameInfo::iterator delete_end = to_delete.end();
00393     for ( ; delete_it != delete_end; ++delete_it )
00394     {
00395       deleteFrame( *delete_it, true );
00396     }
00397   }
00398 
00399   context_->queueRender();
00400 }
00401 
00402 static const Ogre::ColourValue ARROW_HEAD_COLOR(1.0f, 0.1f, 0.6f, 1.0f);
00403 static const Ogre::ColourValue ARROW_SHAFT_COLOR(0.8f, 0.8f, 0.3f, 1.0f);
00404 
00405 FrameInfo* TFDisplay::createFrame(const std::string& frame)
00406 {
00407   FrameInfo* info = new FrameInfo( this );
00408   frames_.insert( std::make_pair( frame, info ) );
00409 
00410   info->name_ = frame;
00411   info->last_update_ = ros::Time::now();
00412   info->axes_ = new Axes( scene_manager_, axes_node_, 0.2, 0.02 );
00413   info->axes_->getSceneNode()->setVisible( show_axes_property_->getBool() );
00414   info->selection_handler_.reset( new FrameSelectionHandler( info, this, context_ ));
00415   info->selection_handler_->addTrackedObjects( info->axes_->getSceneNode() );
00416 
00417   info->name_text_ = new MovableText( frame, "Arial", 0.1 );
00418   info->name_text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_BELOW);
00419   info->name_node_ = names_node_->createChildSceneNode();
00420   info->name_node_->attachObject( info->name_text_ );
00421   info->name_node_->setVisible( show_names_property_->getBool() );
00422 
00423   info->parent_arrow_ = new Arrow( scene_manager_, arrows_node_, 1.0f, 0.01, 1.0f, 0.08 );
00424   info->parent_arrow_->getSceneNode()->setVisible( false );
00425   info->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR);
00426   info->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);
00427 
00428   info->enabled_property_ = new BoolProperty( QString::fromStdString( info->name_ ), true, "Enable or disable this individual frame.",
00429                                               frames_category_, SLOT( updateVisibilityFromFrame() ), info );
00430 
00431   info->parent_property_ = new StringProperty( "Parent", "", "Parent of this frame.  (Not editable)",
00432                                                info->enabled_property_ );
00433   info->parent_property_->setReadOnly( true );
00434 
00435   info->position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
00436                                                  "Position of this frame, in the current Fixed Frame.  (Not editable)",
00437                                                  info->enabled_property_ );
00438   info->position_property_->setReadOnly( true );
00439 
00440   info->orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
00441                                                         "Orientation of this frame, in the current Fixed Frame.  (Not editable)",
00442                                                         info->enabled_property_ );
00443   info->orientation_property_->setReadOnly( true );
00444 
00445   info->rel_position_property_ = new VectorProperty( "Relative Position", Ogre::Vector3::ZERO,
00446                                                  "Position of this frame, relative to it's parent frame.  (Not editable)",
00447                                                  info->enabled_property_ );
00448   info->rel_position_property_->setReadOnly( true );
00449 
00450   info->rel_orientation_property_ = new QuaternionProperty( "Relative Orientation", Ogre::Quaternion::IDENTITY,
00451                                                         "Orientation of this frame, relative to it's parent frame.  (Not editable)",
00452                                                         info->enabled_property_ );
00453   info->rel_orientation_property_->setReadOnly( true );
00454 
00455   updateFrame( info );
00456 
00457   return info;
00458 }
00459 
00460 Ogre::ColourValue lerpColor(const Ogre::ColourValue& start, const Ogre::ColourValue& end, float t)
00461 {
00462   return start * t + end * (1 - t);
00463 }
00464 
00465 void TFDisplay::updateFrame( FrameInfo* frame )
00466 {
00467   tf::TransformListener* tf = context_->getTFClient();
00468 
00469   // Check last received time so we can grey out/fade out frames that have stopped being published
00470   ros::Time latest_time;
00471   tf->getLatestCommonTime( fixed_frame_.toStdString(), frame->name_, latest_time, 0 );
00472   if( latest_time != frame->last_time_to_fixed_ )
00473   {
00474     frame->last_update_ = ros::Time::now();
00475     frame->last_time_to_fixed_ = latest_time;
00476   }
00477 
00478   // Fade from color -> grey, then grey -> fully transparent
00479   ros::Duration age = ros::Time::now() - frame->last_update_;
00480   float frame_timeout = frame_timeout_property_->getFloat();
00481   float one_third_timeout = frame_timeout * 0.3333333f;
00482   if( age > ros::Duration( frame_timeout ))
00483   {
00484     frame->parent_arrow_->getSceneNode()->setVisible(false);
00485     frame->axes_->getSceneNode()->setVisible(false);
00486     frame->name_node_->setVisible(false);
00487     return;
00488   }
00489   else if (age > ros::Duration(one_third_timeout))
00490   {
00491     Ogre::ColourValue grey(0.7, 0.7, 0.7, 1.0);
00492 
00493     if (age > ros::Duration(one_third_timeout * 2))
00494     {
00495       float a = std::max(0.0, (frame_timeout - age.toSec())/one_third_timeout);
00496       Ogre::ColourValue c = Ogre::ColourValue(grey.r, grey.g, grey.b, a);
00497 
00498       frame->axes_->setXColor(c);
00499       frame->axes_->setYColor(c);
00500       frame->axes_->setZColor(c);
00501       frame->name_text_->setColor(c);
00502       frame->parent_arrow_->setColor(c.r, c.g, c.b, c.a);
00503     }
00504     else
00505     {
00506       float t = std::max(0.0, (one_third_timeout * 2 - age.toSec())/one_third_timeout);
00507       frame->axes_->setXColor(lerpColor(frame->axes_->getDefaultXColor(), grey, t));
00508       frame->axes_->setYColor(lerpColor(frame->axes_->getDefaultYColor(), grey, t));
00509       frame->axes_->setZColor(lerpColor(frame->axes_->getDefaultZColor(), grey, t));
00510       frame->name_text_->setColor(lerpColor(Ogre::ColourValue::White, grey, t));
00511       frame->parent_arrow_->setShaftColor(lerpColor(ARROW_SHAFT_COLOR, grey, t));
00512       frame->parent_arrow_->setHeadColor(lerpColor(ARROW_HEAD_COLOR, grey, t));
00513     }
00514   }
00515   else
00516   {
00517     frame->axes_->setToDefaultColors();
00518     frame->name_text_->setColor(Ogre::ColourValue::White);
00519     frame->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR);
00520     frame->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);
00521   }
00522 
00523   setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK");
00524 
00525   Ogre::Vector3 position;
00526   Ogre::Quaternion orientation;
00527   if( !context_->getFrameManager()->getTransform( frame->name_, ros::Time(), position, orientation ))
00528   {
00529     std::stringstream ss;
00530     ss << "No transform from [" << frame->name_ << "] to frame [" << fixed_frame_.toStdString() << "]";
00531     setStatusStd(StatusProperty::Warn, frame->name_, ss.str());
00532     ROS_DEBUG( "Error transforming frame '%s' to frame '%s'", frame->name_.c_str(), qPrintable( fixed_frame_ ));
00533     frame->name_node_->setVisible( false );
00534     frame->axes_->getSceneNode()->setVisible( false );
00535     frame->parent_arrow_->getSceneNode()->setVisible( false );
00536     return;
00537   }
00538 
00539   frame->selection_handler_->setPosition( position );
00540   frame->selection_handler_->setOrientation( orientation );
00541 
00542   bool frame_enabled = frame->enabled_property_->getBool();
00543 
00544   frame->axes_->setPosition( position );
00545   frame->axes_->setOrientation( orientation );
00546   frame->axes_->getSceneNode()->setVisible( show_axes_property_->getBool() && frame_enabled);
00547   float scale = scale_property_->getFloat();
00548   frame->axes_->setScale( Ogre::Vector3( scale, scale, scale ));
00549 
00550   frame->name_node_->setPosition( position );
00551   frame->name_node_->setVisible( show_names_property_->getBool() && frame_enabled );
00552   frame->name_node_->setScale( scale, scale, scale );
00553 
00554   frame->position_property_->setVector( position );
00555   frame->orientation_property_->setQuaternion( orientation );
00556 
00557   std::string old_parent = frame->parent_;
00558   frame->parent_.clear();
00559   bool has_parent = tf->getParent( frame->name_, ros::Time(), frame->parent_ );
00560   if( has_parent )
00561   {
00562     // If this frame has no tree property or the parent has changed,
00563     if( !frame->tree_property_ || old_parent != frame->parent_ )
00564     {
00565       // Look up the new parent.
00566       M_FrameInfo::iterator parent_it = frames_.find( frame->parent_ );
00567       if( parent_it != frames_.end() )
00568       {
00569         FrameInfo* parent = parent_it->second;
00570 
00571         // Delete the old tree property.
00572         delete frame->tree_property_;
00573         frame->tree_property_ = NULL;
00574 
00575         // If the parent has a tree property, make a new tree property for this frame.
00576         if( parent->tree_property_ )
00577         {
00578           frame->tree_property_ = new Property( QString::fromStdString( frame->name_ ), QVariant(), "", parent->tree_property_ );
00579         }
00580       }
00581     }
00582 
00583     tf::StampedTransform transform;
00584     try {
00585       context_->getFrameManager()->getTFClientPtr()->lookupTransform(frame->parent_,frame->name_,ros::Time(0),transform);
00586     }
00587     catch(tf::TransformException& e)
00588     {
00589       ROS_DEBUG( "Error transforming frame '%s' (parent of '%s') to frame '%s'",
00590                  frame->parent_.c_str(), frame->name_.c_str(), qPrintable( fixed_frame_ ));
00591     }
00592 
00593     // get the position/orientation relative to the parent frame
00594     Ogre::Vector3 relative_position( transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z() );
00595     Ogre::Quaternion relative_orientation( transform.getRotation().w(), transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z() );
00596     frame->rel_position_property_->setVector( relative_position );
00597     frame->rel_orientation_property_->setQuaternion( relative_orientation );
00598 
00599     if( show_arrows_property_->getBool() )
00600     {
00601       Ogre::Vector3 parent_position;
00602       Ogre::Quaternion parent_orientation;
00603       if (!context_->getFrameManager()->getTransform(frame->parent_, ros::Time(), parent_position, parent_orientation))
00604       {
00605         ROS_DEBUG( "Error transforming frame '%s' (parent of '%s') to frame '%s'",
00606                    frame->parent_.c_str(), frame->name_.c_str(), qPrintable( fixed_frame_ ));
00607       }
00608 
00609       Ogre::Vector3 direction = parent_position - position;
00610       float distance = direction.length();
00611       direction.normalise();
00612 
00613       Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
00614 
00615       frame->distance_to_parent_ = distance;
00616       float head_length = ( distance < 0.1*scale ) ? (0.1*scale*distance) : 0.1*scale;
00617       float shaft_length = distance - head_length;
00618       // aleeper: This was changed from 0.02 and 0.08 to 0.01 and 0.04 to match proper radius handling in arrow.cpp
00619       frame->parent_arrow_->set( shaft_length, 0.01*scale, head_length, 0.04*scale );
00620 
00621       if ( distance > 0.001f )
00622       {
00623         frame->parent_arrow_->getSceneNode()->setVisible( show_arrows_property_->getBool() && frame_enabled );
00624       }
00625       else
00626       {
00627         frame->parent_arrow_->getSceneNode()->setVisible( false );
00628       }
00629 
00630       frame->parent_arrow_->setPosition( position );
00631       frame->parent_arrow_->setOrientation( orient );
00632     }
00633     else
00634     {
00635       frame->parent_arrow_->getSceneNode()->setVisible( false );
00636     }
00637   }
00638   else
00639   {
00640     if ( !frame->tree_property_ || old_parent != frame->parent_ )
00641     {
00642       delete frame->tree_property_;
00643       frame->tree_property_ = new Property( QString::fromStdString( frame->name_ ), QVariant(), "", tree_category_ );
00644     }
00645 
00646     frame->parent_arrow_->getSceneNode()->setVisible( false );
00647   }
00648 
00649   frame->parent_property_->setStdString( frame->parent_ );
00650   frame->selection_handler_->setParentName( frame->parent_ );
00651 }
00652 
00653 void TFDisplay::deleteFrame( FrameInfo* frame, bool delete_properties )
00654 {
00655   M_FrameInfo::iterator it = frames_.find( frame->name_ );
00656   ROS_ASSERT( it != frames_.end() );
00657 
00658   frames_.erase( it );
00659 
00660   delete frame->axes_;
00661   context_->getSelectionManager()->removeObject( frame->axes_coll_ );
00662   delete frame->parent_arrow_;
00663   delete frame->name_text_;
00664   scene_manager_->destroySceneNode( frame->name_node_->getName() );
00665   if( delete_properties )
00666   {
00667     delete frame->enabled_property_;
00668     delete frame->tree_property_;
00669   }
00670   delete frame;
00671 }
00672 
00673 void TFDisplay::fixedFrameChanged()
00674 {
00675   update_timer_ = update_rate_property_->getFloat();
00676 }
00677 
00678 void TFDisplay::reset()
00679 {
00680   Display::reset();
00681   clear();
00682 }
00683 
00685 // FrameInfo
00686 
00687 FrameInfo::FrameInfo( TFDisplay* display )
00688   : display_( display )
00689   , axes_( NULL )
00690   , axes_coll_( 0 )
00691   , parent_arrow_( NULL )
00692   , name_text_( NULL )
00693   , distance_to_parent_( 0.0f )
00694   , arrow_orientation_(Ogre::Quaternion::IDENTITY)
00695   , tree_property_( NULL )
00696 {}
00697 
00698 void FrameInfo::updateVisibilityFromFrame()
00699 {
00700   bool enabled = enabled_property_->getBool();
00701   selection_handler_->setEnabled( enabled );
00702   setEnabled( enabled );
00703 }
00704 
00705 void FrameInfo::updateVisibilityFromSelection()
00706 {
00707   bool enabled = selection_handler_->getEnabled();
00708   enabled_property_->setBool( enabled );
00709   setEnabled( enabled );
00710 }
00711 
00712 void FrameInfo::setEnabled( bool enabled )
00713 {
00714   if( name_node_ )
00715   {
00716     name_node_->setVisible( display_->show_names_property_->getBool() && enabled );
00717   }
00718 
00719   if( axes_ )
00720   {
00721     axes_->getSceneNode()->setVisible( display_->show_axes_property_->getBool() && enabled );
00722   }
00723 
00724   if( parent_arrow_ )
00725   {
00726     if( distance_to_parent_ > 0.001f )
00727     {
00728       parent_arrow_->getSceneNode()->setVisible( display_->show_arrows_property_->getBool() && enabled );
00729     }
00730     else
00731     {
00732       parent_arrow_->getSceneNode()->setVisible( false );
00733     }
00734   }
00735 
00736   if( display_->all_enabled_property_->getBool() && !enabled)
00737   {
00738     display_->changing_single_frame_enabled_state_ = true;
00739     display_->all_enabled_property_->setBool( false );
00740     display_->changing_single_frame_enabled_state_ = false;
00741   }
00742 
00743   display_->context_->queueRender();
00744 }
00745 
00746 } // namespace rviz
00747 
00748 #include <pluginlib/class_list_macros.h>
00749 PLUGINLIB_EXPORT_CLASS( rviz::TFDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:28