00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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_;
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;
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
00219 tree_category_->removeChildren();
00220
00221
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
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
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
00563 if( !frame->tree_property_ || old_parent != frame->parent_ )
00564 {
00565
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
00572 delete frame->tree_property_;
00573 frame->tree_property_ = NULL;
00574
00575
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
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
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
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 }
00747
00748 #include <pluginlib/class_list_macros.h>
00749 PLUGINLIB_EXPORT_CLASS( rviz::TFDisplay, rviz::Display )