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 "tf_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/selection/selection_manager.h"
00033 #include "rviz/selection/forwards.h"
00034 #include "rviz/properties/property.h"
00035 #include "rviz/properties/property_manager.h"
00036 #include "rviz/frame_manager.h"
00037
00038 #include <ogre_tools/arrow.h>
00039 #include <ogre_tools/axes.h>
00040 #include <ogre_tools/movable_text.h>
00041
00042 #include <boost/bind.hpp>
00043
00044 #include <OGRE/OgreSceneNode.h>
00045 #include <OGRE/OgreSceneManager.h>
00046
00047 #include <tf/transform_listener.h>
00048
00049
00050 namespace rviz
00051 {
00052
00053 struct FrameInfo
00054 {
00055 FrameInfo();
00056
00057 const Ogre::Vector3& getPositionInRobotSpace() { return robot_space_position_; }
00058 const Ogre::Quaternion& getOrientationInRobotSpace() { return robot_space_orientation_; }
00059 const std::string& getParent() { return parent_; }
00060
00061 bool isEnabled() { return enabled_; }
00062
00063 std::string name_;
00064 std::string parent_;
00065 ogre_tools::Axes* axes_;
00066 CollObjectHandle axes_coll_;
00067 ogre_tools::Arrow* parent_arrow_;
00068 ogre_tools::MovableText* name_text_;
00069 Ogre::SceneNode* name_node_;
00070
00071 Ogre::Vector3 position_;
00072 Ogre::Quaternion orientation_;
00073 float distance_to_parent_;
00074 Ogre::Quaternion arrow_orientation_;
00075
00076 Ogre::Vector3 robot_space_position_;
00077 Ogre::Quaternion robot_space_orientation_;
00078
00079 bool enabled_;
00080
00081 ros::Time last_update_;
00082 ros::Time last_time_to_fixed_;
00083
00084 CategoryPropertyWPtr category_;
00085 Vector3PropertyWPtr position_property_;
00086 QuaternionPropertyWPtr orientation_property_;
00087 StringPropertyWPtr parent_property_;
00088 BoolPropertyWPtr enabled_property_;
00089
00090 CategoryPropertyWPtr tree_property_;
00091 };
00092
00093 FrameInfo::FrameInfo()
00094 : axes_( NULL )
00095 , axes_coll_(NULL)
00096 , parent_arrow_( NULL )
00097 , name_text_( NULL )
00098 , position_(Ogre::Vector3::ZERO)
00099 , orientation_(Ogre::Quaternion::IDENTITY)
00100 , distance_to_parent_( 0.0f )
00101 , arrow_orientation_(Ogre::Quaternion::IDENTITY)
00102 , robot_space_position_(Ogre::Vector3::ZERO)
00103 , robot_space_orientation_(Ogre::Quaternion::IDENTITY)
00104 , enabled_(true)
00105 {
00106
00107 }
00108
00109 void TFDisplay::setFrameEnabled(FrameInfo* frame, bool enabled)
00110 {
00111 frame->enabled_ = enabled;
00112
00113 propertyChanged(frame->enabled_property_);
00114
00115 if (frame->name_node_)
00116 {
00117 frame->name_node_->setVisible(show_names_ && enabled);
00118 }
00119
00120 if (frame->axes_)
00121 {
00122 frame->axes_->getSceneNode()->setVisible(show_axes_ && enabled);
00123 }
00124
00125 if (frame->parent_arrow_)
00126 {
00127 if (frame->distance_to_parent_ > 0.001f)
00128 {
00129 frame->parent_arrow_->getSceneNode()->setVisible(show_arrows_ && enabled);
00130 }
00131 else
00132 {
00133 frame->parent_arrow_->getSceneNode()->setVisible(false);
00134 }
00135 }
00136
00137 if (all_enabled_ && !enabled)
00138 {
00139 all_enabled_ = false;
00140
00141 propertyChanged(all_enabled_property_);
00142 }
00143
00144 causeRender();
00145 }
00146
00147 class FrameSelectionHandler : public SelectionHandler
00148 {
00149 public:
00150 FrameSelectionHandler(FrameInfo* frame, TFDisplay* display);
00151 virtual ~FrameSelectionHandler();
00152
00153 virtual void createProperties(const Picked& obj, PropertyManager* property_manager);
00154
00155 private:
00156 FrameInfo* frame_;
00157 TFDisplay* display_;
00158 };
00159
00160 FrameSelectionHandler::FrameSelectionHandler(FrameInfo* frame, TFDisplay* display)
00161 : frame_(frame)
00162 , display_(display)
00163 {
00164 }
00165
00166 FrameSelectionHandler::~FrameSelectionHandler()
00167 {
00168 }
00169
00170 void FrameSelectionHandler::createProperties(const Picked& obj, PropertyManager* property_manager)
00171 {
00172 std::stringstream ss;
00173 ss << frame_->name_ << " Frame " << frame_->name_;
00174
00175 CategoryPropertyWPtr cat = property_manager->createCategory( "Frame " + frame_->name_, ss.str(), CategoryPropertyWPtr() );
00176 properties_.push_back(cat);
00177
00178 properties_.push_back(property_manager->createProperty<BoolProperty>( "Enabled", ss.str(), boost::bind( &FrameInfo::isEnabled, frame_ ),
00179 boost::bind( &TFDisplay::setFrameEnabled, display_, frame_, _1 ), cat, this ));
00180 properties_.push_back(property_manager->createProperty<StringProperty>( "Parent", ss.str(), boost::bind( &FrameInfo::getParent, frame_ ),
00181 StringProperty::Setter(), cat, this ));
00182 properties_.push_back(property_manager->createProperty<Vector3Property>( "Position", ss.str(), boost::bind( &FrameInfo::getPositionInRobotSpace, frame_ ),
00183 Vector3Property::Setter(), cat, this ));
00184 properties_.push_back(property_manager->createProperty<QuaternionProperty>( "Orientation", ss.str(), boost::bind( &FrameInfo::getOrientationInRobotSpace, frame_ ),
00185 QuaternionProperty::Setter(), cat, this ));
00186 }
00187
00188 typedef std::set<FrameInfo*> S_FrameInfo;
00189
00190 TFDisplay::TFDisplay()
00191 : Display()
00192 , update_timer_( 0.0f )
00193 , update_rate_( 0.0f )
00194 , show_names_( true )
00195 , show_arrows_( true )
00196 , show_axes_( true )
00197 , frame_timeout_(15.0f)
00198 , all_enabled_(true)
00199 , scale_( 1 )
00200 {
00201 }
00202
00203 TFDisplay::~TFDisplay()
00204 {
00205 clear();
00206
00207 root_node_->removeAndDestroyAllChildren();
00208 scene_manager_->destroySceneNode( root_node_->getName() );
00209 }
00210
00211 void TFDisplay::onInitialize()
00212 {
00213 root_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00214
00215 names_node_ = root_node_->createChildSceneNode();
00216 arrows_node_ = root_node_->createChildSceneNode();
00217 axes_node_ = root_node_->createChildSceneNode();
00218 }
00219
00220 void TFDisplay::clear()
00221 {
00222 property_manager_->deleteChildren(tree_category_.lock());
00223 property_manager_->deleteChildren(frames_category_.lock());
00224
00225 S_FrameInfo to_delete;
00226 M_FrameInfo::iterator frame_it = frames_.begin();
00227 M_FrameInfo::iterator frame_end = frames_.end();
00228 for ( ; frame_it != frame_end; ++frame_it )
00229 {
00230 to_delete.insert( frame_it->second );
00231 }
00232
00233 S_FrameInfo::iterator delete_it = to_delete.begin();
00234 S_FrameInfo::iterator delete_end = to_delete.end();
00235 for ( ; delete_it != delete_end; ++delete_it )
00236 {
00237 deleteFrame( *delete_it, false );
00238 }
00239
00240 frames_.clear();
00241
00242 update_timer_ = 0.0f;
00243
00244 clearStatuses();
00245 }
00246
00247 void TFDisplay::onEnable()
00248 {
00249 root_node_->setVisible( true );
00250
00251 names_node_->setVisible( show_names_ );
00252 arrows_node_->setVisible( show_arrows_ );
00253 axes_node_->setVisible( show_axes_ );
00254 }
00255
00256 void TFDisplay::onDisable()
00257 {
00258 root_node_->setVisible( false );
00259 clear();
00260 }
00261
00262 void TFDisplay::setShowNames( bool show )
00263 {
00264 show_names_ = show;
00265
00266 names_node_->setVisible( show );
00267
00268 M_FrameInfo::iterator it = frames_.begin();
00269 M_FrameInfo::iterator end = frames_.end();
00270 for (; it != end; ++it)
00271 {
00272 FrameInfo* frame = it->second;
00273
00274 setFrameEnabled(frame, frame->enabled_);
00275 }
00276
00277 propertyChanged(show_names_property_);
00278 }
00279
00280 void TFDisplay::setShowAxes( bool show )
00281 {
00282 show_axes_ = show;
00283
00284 axes_node_->setVisible( show );
00285
00286 M_FrameInfo::iterator it = frames_.begin();
00287 M_FrameInfo::iterator end = frames_.end();
00288 for (; it != end; ++it)
00289 {
00290 FrameInfo* frame = it->second;
00291
00292 setFrameEnabled(frame, frame->enabled_);
00293 }
00294
00295 propertyChanged(show_axes_property_);
00296 }
00297
00298 void TFDisplay::setShowArrows( bool show )
00299 {
00300 show_arrows_ = show;
00301
00302 arrows_node_->setVisible( show );
00303
00304 M_FrameInfo::iterator it = frames_.begin();
00305 M_FrameInfo::iterator end = frames_.end();
00306 for (; it != end; ++it)
00307 {
00308 FrameInfo* frame = it->second;
00309
00310 setFrameEnabled(frame, frame->enabled_);
00311 }
00312
00313 propertyChanged(show_arrows_property_);
00314 }
00315
00316 void TFDisplay::setAllEnabled(bool enabled)
00317 {
00318 all_enabled_ = enabled;
00319
00320 M_FrameInfo::iterator it = frames_.begin();
00321 M_FrameInfo::iterator end = frames_.end();
00322 for (; it != end; ++it)
00323 {
00324 FrameInfo* frame = it->second;
00325
00326 setFrameEnabled(frame, enabled);
00327 }
00328
00329 propertyChanged(all_enabled_property_);
00330 }
00331
00332 void TFDisplay::setFrameTimeout(float timeout)
00333 {
00334 frame_timeout_ = timeout;
00335 propertyChanged(frame_timeout_property_);
00336 }
00337
00338 void TFDisplay::setScale(float scale)
00339 {
00340 scale_ = scale;
00341 propertyChanged(scale_property_);
00342 }
00343
00344 void TFDisplay::setUpdateRate( float rate )
00345 {
00346 update_rate_ = rate;
00347
00348 propertyChanged(update_rate_property_);
00349 }
00350
00351 void TFDisplay::update(float wall_dt, float ros_dt)
00352 {
00353 update_timer_ += wall_dt;
00354 if ( update_rate_ < 0.0001f || update_timer_ > update_rate_ )
00355 {
00356 updateFrames();
00357
00358 update_timer_ = 0.0f;
00359 }
00360 }
00361
00362 FrameInfo* TFDisplay::getFrameInfo( const std::string& frame )
00363 {
00364 M_FrameInfo::iterator it = frames_.find( frame );
00365 if ( it == frames_.end() )
00366 {
00367 return NULL;
00368 }
00369
00370 return it->second;
00371 }
00372
00373 void TFDisplay::updateFrames()
00374 {
00375 typedef std::vector<std::string> V_string;
00376 V_string frames;
00377 vis_manager_->getTFClient()->getFrameStrings( frames );
00378 std::sort(frames.begin(), frames.end());
00379
00380 S_FrameInfo current_frames;
00381
00382 {
00383 V_string::iterator it = frames.begin();
00384 V_string::iterator end = frames.end();
00385 for ( ; it != end; ++it )
00386 {
00387 const std::string& frame = *it;
00388
00389 if ( frame.empty() )
00390 {
00391 continue;
00392 }
00393
00394 FrameInfo* info = getFrameInfo( frame );
00395 if (!info)
00396 {
00397 info = createFrame(frame);
00398 }
00399 else
00400 {
00401 updateFrame(info);
00402 }
00403
00404 current_frames.insert( info );
00405 }
00406 }
00407
00408 {
00409 S_FrameInfo to_delete;
00410 M_FrameInfo::iterator frame_it = frames_.begin();
00411 M_FrameInfo::iterator frame_end = frames_.end();
00412 for ( ; frame_it != frame_end; ++frame_it )
00413 {
00414 if ( current_frames.find( frame_it->second ) == current_frames.end() )
00415 {
00416 to_delete.insert( frame_it->second );
00417 }
00418 }
00419
00420 S_FrameInfo::iterator delete_it = to_delete.begin();
00421 S_FrameInfo::iterator delete_end = to_delete.end();
00422 for ( ; delete_it != delete_end; ++delete_it )
00423 {
00424 deleteFrame( *delete_it, true );
00425 }
00426 }
00427
00428 causeRender();
00429 }
00430
00431 static const Ogre::ColourValue ARROW_HEAD_COLOR(1.0f, 0.1f, 0.6f, 1.0f);
00432 static const Ogre::ColourValue ARROW_SHAFT_COLOR(0.8f, 0.8f, 0.3f, 1.0f);
00433
00434 FrameInfo* TFDisplay::createFrame(const std::string& frame)
00435 {
00436 FrameInfo* info = new FrameInfo;
00437 frames_.insert( std::make_pair( frame, info ) );
00438
00439 info->name_ = frame;
00440 info->last_update_ = ros::Time::now();
00441 info->axes_ = new ogre_tools::Axes( scene_manager_, axes_node_, 0.2, 0.02 );
00442 info->axes_->getSceneNode()->setVisible( show_axes_ );
00443 info->axes_coll_ = vis_manager_->getSelectionManager()->createCollisionForObject(info->axes_, SelectionHandlerPtr(new FrameSelectionHandler(info, this)));
00444
00445 info->name_text_ = new ogre_tools::MovableText( frame, "Arial", 0.1 );
00446 info->name_text_->setTextAlignment(ogre_tools::MovableText::H_CENTER, ogre_tools::MovableText::V_BELOW);
00447 info->name_node_ = names_node_->createChildSceneNode();
00448 info->name_node_->attachObject( info->name_text_ );
00449 info->name_node_->setVisible( show_names_ );
00450
00451 info->parent_arrow_ = new ogre_tools::Arrow( scene_manager_, arrows_node_, 1.0f, 0.01, 1.0f, 0.08 );
00452 info->parent_arrow_->getSceneNode()->setVisible( false );
00453 info->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR);
00454 info->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);
00455
00456 info->enabled_ = true;
00457
00458 std::string prefix = property_prefix_ + "Frames.";
00459
00460 info->category_ = property_manager_->createCategory( info->name_, prefix, frames_category_, this );
00461
00462 prefix += info->name_ + ".";
00463
00464 info->enabled_property_ = property_manager_->createProperty<BoolProperty>( "Enabled", prefix, boost::bind( &FrameInfo::isEnabled, info ),
00465 boost::bind( &TFDisplay::setFrameEnabled, this, info, _1 ), info->category_, this );
00466 setPropertyHelpText(info->enabled_property_, "Enable or disable this individual frame.");
00467 info->parent_property_ = property_manager_->createProperty<StringProperty>( "Parent", prefix, boost::bind( &FrameInfo::getParent, info ),
00468 StringProperty::Setter(), info->category_, this );
00469 setPropertyHelpText(info->parent_property_, "Parent of this frame. (Not editable)");
00470 info->position_property_ = property_manager_->createProperty<Vector3Property>( "Position", prefix, boost::bind( &FrameInfo::getPositionInRobotSpace, info ),
00471 Vector3Property::Setter(), info->category_, this );
00472 setPropertyHelpText(info->position_property_, "Position of this frame, in the current Fixed Frame. (Not editable)");
00473 info->orientation_property_ = property_manager_->createProperty<QuaternionProperty>( "Orientation", prefix, boost::bind( &FrameInfo::getOrientationInRobotSpace, info ),
00474 QuaternionProperty::Setter(), info->category_, this );
00475 setPropertyHelpText(info->orientation_property_, "Orientation of this frame, in the current Fixed Frame. (Not editable)");
00476 updateFrame( info );
00477
00478 return info;
00479 }
00480
00481 Ogre::ColourValue lerpColor(const Ogre::ColourValue& start, const Ogre::ColourValue& end, float t)
00482 {
00483 return start * t + end * (1 - t);
00484 }
00485
00486 void TFDisplay::updateFrame(FrameInfo* frame)
00487 {
00488 tf::TransformListener* tf = vis_manager_->getTFClient();
00489
00490
00491 ros::Time latest_time;
00492 tf->getLatestCommonTime(fixed_frame_, frame->name_, latest_time, 0);
00493 if (latest_time != frame->last_time_to_fixed_)
00494 {
00495 frame->last_update_ = ros::Time::now();
00496 frame->last_time_to_fixed_ = latest_time;
00497 }
00498
00499
00500 ros::Duration age = ros::Time::now() - frame->last_update_;
00501 float one_third_timeout = frame_timeout_ * 0.3333333f;
00502 if (age > ros::Duration(frame_timeout_))
00503 {
00504 frame->parent_arrow_->getSceneNode()->setVisible(false);
00505 frame->axes_->getSceneNode()->setVisible(false);
00506 frame->name_node_->setVisible(false);
00507 return;
00508 }
00509 else if (age > ros::Duration(one_third_timeout))
00510 {
00511 Ogre::ColourValue grey(0.7, 0.7, 0.7, 1.0);
00512
00513 if (age > ros::Duration(one_third_timeout * 2))
00514 {
00515 float a = std::max(0.0, (frame_timeout_ - age.toSec())/one_third_timeout);
00516 Ogre::ColourValue c = Ogre::ColourValue(grey.r, grey.g, grey.b, a);
00517
00518 frame->axes_->setXColor(c);
00519 frame->axes_->setYColor(c);
00520 frame->axes_->setZColor(c);
00521 frame->name_text_->setColor(c);
00522 frame->parent_arrow_->setColor(c.r, c.g, c.b, c.a);
00523 }
00524 else
00525 {
00526 float t = std::max(0.0, (one_third_timeout * 2 - age.toSec())/one_third_timeout);
00527 frame->axes_->setXColor(lerpColor(frame->axes_->getDefaultXColor(), grey, t));
00528 frame->axes_->setYColor(lerpColor(frame->axes_->getDefaultYColor(), grey, t));
00529 frame->axes_->setZColor(lerpColor(frame->axes_->getDefaultZColor(), grey, t));
00530 frame->name_text_->setColor(lerpColor(Ogre::ColourValue::White, grey, t));
00531 frame->parent_arrow_->setShaftColor(lerpColor(ARROW_SHAFT_COLOR, grey, t));
00532 frame->parent_arrow_->setHeadColor(lerpColor(ARROW_HEAD_COLOR, grey, t));
00533 }
00534 }
00535 else
00536 {
00537 frame->axes_->setToDefaultColors();
00538 frame->name_text_->setColor(Ogre::ColourValue::White);
00539 frame->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR);
00540 frame->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);
00541 }
00542
00543 setStatus(status_levels::Ok, frame->name_, "Transform OK");
00544
00545 if (!vis_manager_->getFrameManager()->getTransform(frame->name_, ros::Time(), frame->position_, frame->orientation_))
00546 {
00547 std::stringstream ss;
00548 ss << "No transform from [" << frame->name_ << "] to frame [" << fixed_frame_ << "]";
00549 setStatus(status_levels::Warn, frame->name_, ss.str());
00550 ROS_DEBUG("Error transforming frame '%s' to frame '%s'", frame->name_.c_str(), fixed_frame_.c_str());
00551 }
00552
00553 frame->robot_space_position_ = frame->position_;
00554 frame->robot_space_orientation_ = frame->orientation_;
00555
00556 frame->axes_->setPosition( frame->position_ );
00557 frame->axes_->setOrientation( frame->orientation_ );
00558 frame->axes_->getSceneNode()->setVisible(show_axes_ && frame->enabled_);
00559 frame->axes_->setScale( Ogre::Vector3(scale_,scale_,scale_) );
00560
00561 frame->name_node_->setPosition( frame->position_ );
00562 frame->name_node_->setVisible(show_names_ && frame->enabled_);
00563 frame->name_node_->setScale(scale_,scale_,scale_);
00564
00565 propertyChanged(frame->position_property_);
00566 propertyChanged(frame->orientation_property_);
00567
00568 std::string old_parent = frame->parent_;
00569 frame->parent_.clear();
00570 bool has_parent = tf->getParent( frame->name_, ros::Time(), frame->parent_ );
00571 if ( has_parent )
00572 {
00573 {
00574 CategoryPropertyPtr cat_prop = frame->tree_property_.lock();
00575 if ( !cat_prop || old_parent != frame->parent_ )
00576 {
00577 M_FrameInfo::iterator parent_it = frames_.find( frame->parent_ );
00578
00579 if ( parent_it != frames_.end() )
00580 {
00581 FrameInfo* parent = parent_it->second;
00582
00583 property_manager_->deleteProperty( cat_prop );
00584 cat_prop.reset();
00585
00586 if ( parent->tree_property_.lock() )
00587 {
00588 frame->tree_property_ = property_manager_->createCategory( frame->name_, property_prefix_ + "Tree.", parent->tree_property_, this );
00589 }
00590 }
00591 }
00592 }
00593
00594 if ( show_arrows_ )
00595 {
00596 Ogre::Vector3 parent_position;
00597 Ogre::Quaternion parent_orientation;
00598 if (!vis_manager_->getFrameManager()->getTransform(frame->parent_, ros::Time(), parent_position, parent_orientation))
00599 {
00600 ROS_DEBUG("Error transforming frame '%s' (parent of '%s') to frame '%s'", frame->parent_.c_str(), frame->name_.c_str(), fixed_frame_.c_str());
00601 }
00602
00603 Ogre::Vector3 direction = parent_position - frame->position_;
00604 float distance = direction.length();
00605 direction.normalise();
00606
00607 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
00608
00609 Ogre::Vector3 old_pos = frame->parent_arrow_->getPosition();
00610
00611 frame->distance_to_parent_ = distance;
00612 float head_length = ( distance < 0.1*scale_ ) ? (0.1*scale_*distance) : 0.1*scale_;
00613 float shaft_length = distance - head_length;
00614 frame->parent_arrow_->set( shaft_length, 0.02*scale_, head_length, 0.08*scale_ );
00615
00616 if ( distance > 0.001f )
00617 {
00618 frame->parent_arrow_->getSceneNode()->setVisible( show_arrows_ && frame->enabled_ );
00619 }
00620 else
00621 {
00622 frame->parent_arrow_->getSceneNode()->setVisible( false );
00623 }
00624
00625 frame->parent_arrow_->setPosition( frame->position_ );
00626 frame->parent_arrow_->setOrientation( orient );
00627 }
00628 else
00629 {
00630 frame->parent_arrow_->getSceneNode()->setVisible( false );
00631 }
00632 }
00633 else
00634 {
00635 CategoryPropertyPtr tree_prop = frame->tree_property_.lock();
00636 if ( !tree_prop || old_parent != frame->parent_ )
00637 {
00638 property_manager_->deleteProperty( tree_prop );
00639 frame->tree_property_ = property_manager_->createCategory( frame->name_, property_prefix_ + "Tree.", tree_category_, this );
00640 }
00641
00642 frame->parent_arrow_->getSceneNode()->setVisible( false );
00643 }
00644
00645 propertyChanged(frame->parent_property_);
00646 }
00647
00648 void TFDisplay::deleteFrame(FrameInfo* frame, bool delete_properties)
00649 {
00650 M_FrameInfo::iterator it = frames_.find( frame->name_ );
00651 ROS_ASSERT( it != frames_.end() );
00652
00653 frames_.erase( it );
00654
00655 delete frame->axes_;
00656 vis_manager_->getSelectionManager()->removeObject(frame->axes_coll_);
00657 delete frame->parent_arrow_;
00658 delete frame->name_text_;
00659 scene_manager_->destroySceneNode( frame->name_node_->getName() );
00660 if( delete_properties )
00661 {
00662 property_manager_->deleteProperty( frame->category_.lock() );
00663 property_manager_->deleteProperty( frame->tree_property_.lock() );
00664 }
00665 delete frame;
00666 }
00667
00668 void TFDisplay::createProperties()
00669 {
00670 show_names_property_ = property_manager_->createProperty<BoolProperty>( "Show Names", property_prefix_, boost::bind( &TFDisplay::getShowNames, this ),
00671 boost::bind( &TFDisplay::setShowNames, this, _1 ), parent_category_, this );
00672 setPropertyHelpText(show_names_property_, "Whether or not names should be shown next to the frames.");
00673 show_axes_property_ = property_manager_->createProperty<BoolProperty>( "Show Axes", property_prefix_, boost::bind( &TFDisplay::getShowAxes, this ),
00674 boost::bind( &TFDisplay::setShowAxes, this, _1 ), parent_category_, this );
00675 setPropertyHelpText(show_axes_property_, "Whether or not the axes of each frame should be shown.");
00676 show_arrows_property_ = property_manager_->createProperty<BoolProperty>( "Show Arrows", property_prefix_, boost::bind( &TFDisplay::getShowArrows, this ),
00677 boost::bind( &TFDisplay::setShowArrows, this, _1 ), parent_category_, this );
00678 setPropertyHelpText(show_arrows_property_, "Whether or not arrows from child to parent should be shown.");
00679 scale_property_ = property_manager_->createProperty<FloatProperty>( "Marker Scale", property_prefix_, boost::bind( &TFDisplay::getScale, this ),
00680 boost::bind( &TFDisplay::setScale, this, _1 ), parent_category_, this );
00681 setPropertyHelpText(scale_property_, "Scaling factor for all names, axes and arrows.");
00682 update_rate_property_ = property_manager_->createProperty<FloatProperty>( "Update Interval", property_prefix_, boost::bind( &TFDisplay::getUpdateRate, this ),
00683 boost::bind( &TFDisplay::setUpdateRate, this, _1 ), parent_category_, this );
00684 setPropertyHelpText(update_rate_property_, "The interval, in seconds, at which to update the frame transforms. 0 means to do so every update cycle.");
00685 FloatPropertyPtr float_prop = update_rate_property_.lock();
00686 float_prop->setMin( 0.0 );
00687 float_prop->addLegacyName("Update Rate");
00688
00689 frame_timeout_property_ = property_manager_->createProperty<FloatProperty>( "Frame Timeout", property_prefix_, boost::bind( &TFDisplay::getFrameTimeout, this ),
00690 boost::bind( &TFDisplay::setFrameTimeout, this, _1 ), parent_category_, this );
00691 setPropertyHelpText(frame_timeout_property_, "The length of time, in seconds, before a frame that has not been updated is considered \"dead\". For 1/3 of this time"
00692 " the frame will appear correct, for the second 1/3rd it will fade to gray, and then it will fade out completely.");
00693 float_prop = frame_timeout_property_.lock();
00694 float_prop->setMin( 1.0 );
00695
00696 frames_category_ = property_manager_->createCategory( "Frames", property_prefix_, parent_category_, this );
00697 setPropertyHelpText(frames_category_, "The list of all frames.");
00698 CategoryPropertyPtr cat_prop = frames_category_.lock();
00699 cat_prop->collapse();
00700
00701 all_enabled_property_ = property_manager_->createProperty<BoolProperty>( "All Enabled", property_prefix_, boost::bind( &TFDisplay::getAllEnabled, this ),
00702 boost::bind( &TFDisplay::setAllEnabled, this, _1 ), frames_category_, this );
00703 setPropertyHelpText(all_enabled_property_, "Whether all the frames should be enabled or not.");
00704
00705 tree_category_ = property_manager_->createCategory( "Tree", property_prefix_, parent_category_, this );
00706 setPropertyHelpText(tree_category_, "A tree-view of the frames, showing the parent/child relationships.");
00707 cat_prop = tree_category_.lock();
00708 cat_prop->collapse();
00709 }
00710
00711 void TFDisplay::targetFrameChanged()
00712 {
00713 update_timer_ = update_rate_;
00714 }
00715
00716 void TFDisplay::reset()
00717 {
00718 Display::reset();
00719 clear();
00720 }
00721
00722 }
00723