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