tf_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <boost/bind.hpp>
31 
32 #include <OgreSceneNode.h>
33 #include <OgreSceneManager.h>
34 
35 #include <tf/transform_listener.h>
36 
37 #include "rviz/display_context.h"
38 #include "rviz/frame_manager.h"
40 #include "rviz/ogre_helpers/axes.h"
49 
51 
52 namespace rviz
53 {
54 
56 {
57 public:
58  FrameSelectionHandler( FrameInfo* frame, TFDisplay* display, DisplayContext* context );
60 
61  virtual void createProperties( const Picked& obj, Property* parent_property );
62  virtual void destroyProperties( const Picked& obj, Property* parent_property );
63 
64  bool getEnabled();
65  void setEnabled( bool enabled );
66  void setParentName( std::string parent_name );
67  void setPosition( const Ogre::Vector3& position );
68  void setOrientation( const Ogre::Quaternion& orientation );
69 
70 private:
78 };
79 
81  : SelectionHandler( context )
82  , frame_( frame )
83  , display_( display )
89 {
90 }
91 
92 void FrameSelectionHandler::createProperties( const Picked& obj, Property* parent_property )
93 {
94  category_property_ = new Property( "Frame " + QString::fromStdString( frame_->name_ ), QVariant(), "", parent_property );
95 
96  enabled_property_ = new BoolProperty( "Enabled", true, "", category_property_, SLOT( updateVisibilityFromSelection() ), frame_ );
97 
98  parent_property_ = new StringProperty( "Parent", "", "", category_property_ );
100 
101  position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, "", category_property_ );
103 
104  orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, "", category_property_ );
106 }
107 
108 void FrameSelectionHandler::destroyProperties( const Picked& obj, Property* parent_property )
109 {
110  delete category_property_; // This deletes its children as well.
116 }
117 
119 {
120  if( enabled_property_ )
121  {
122  return enabled_property_->getBool();
123  }
124  return false; // should never happen, but don't want to crash if it does.
125 }
126 
128 {
129  if( enabled_property_ )
130  {
131  enabled_property_->setBool( enabled );
132  }
133 }
134 
135 void FrameSelectionHandler::setParentName( std::string parent_name )
136 {
137  if( parent_property_ )
138  {
139  parent_property_->setStdString( parent_name );
140  }
141 }
142 
143 void FrameSelectionHandler::setPosition( const Ogre::Vector3& position )
144 {
145  if( position_property_ )
146  {
147  position_property_->setVector( position );
148  }
149 }
150 
151 void FrameSelectionHandler::setOrientation( const Ogre::Quaternion& orientation )
152 {
154  {
155  orientation_property_->setQuaternion( orientation );
156  }
157 }
158 
159 typedef std::set<FrameInfo*> S_FrameInfo;
160 
162  : Display()
163  , update_timer_( 0.0f )
164  , changing_single_frame_enabled_state_( false )
165 {
166  show_names_property_ = new BoolProperty( "Show Names", true, "Whether or not names should be shown next to the frames.",
167  this, SLOT( updateShowNames() ));
168 
169  show_axes_property_ = new BoolProperty( "Show Axes", true, "Whether or not the axes of each frame should be shown.",
170  this, SLOT( updateShowAxes() ));
171 
172  show_arrows_property_ = new BoolProperty( "Show Arrows", true, "Whether or not arrows from child to parent should be shown.",
173  this, SLOT( updateShowArrows() ));
174 
175  scale_property_ = new FloatProperty( "Marker Scale", 1, "Scaling factor for all names, axes and arrows.", this );
176 
177  update_rate_property_ = new FloatProperty( "Update Interval", 0,
178  "The interval, in seconds, at which to update the frame transforms. 0 means to do so every update cycle.",
179  this );
181 
182  frame_timeout_property_ = new FloatProperty( "Frame Timeout", 15,
183  "The length of time, in seconds, before a frame that has not been updated is considered \"dead\"."
184  " For 1/3 of this time the frame will appear correct, for the second 1/3rd it will fade to gray,"
185  " and then it will fade out completely.",
186  this );
188 
189  frames_category_ = new Property( "Frames", QVariant(), "The list of all frames.", this );
190 
191  all_enabled_property_ = new BoolProperty( "All Enabled", true,
192  "Whether all the frames should be enabled or not.",
193  frames_category_, SLOT( allEnabledChanged() ), this );
194 
195  tree_category_ = new Property( "Tree", QVariant(), "A tree-view of the frames, showing the parent/child relationships.", this );
196 }
197 
199 {
200  if ( initialized() )
201  {
202  root_node_->removeAndDestroyAllChildren();
203  scene_manager_->destroySceneNode( root_node_->getName() );
204  }
205 }
206 
208 {
210 
211  root_node_ = scene_node_->createChildSceneNode();
212 
213  names_node_ = root_node_->createChildSceneNode();
214  arrows_node_ = root_node_->createChildSceneNode();
215  axes_node_ = root_node_->createChildSceneNode();
216 }
217 
218 void TFDisplay::load(const Config& config)
219 {
220  Display::load(config);
221 
222  // Load the enabled state for all frames specified in the config, and store
223  // the values in a map so that the enabled state can be properly set once
224  // the frame is created
225  Config c = config.mapGetChild("Frames");
226  for( Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance() )
227  {
228  QString key = iter.currentKey();
229  if( key != "All Enabled" )
230  {
231  const Config& child = iter.currentChild();
232  bool enabled = child.mapGetChild("Value").getValue().toBool();
233 
234  frame_config_enabled_state_[key.toStdString()] = enabled;
235  }
236  }
237 }
238 
240 {
241  // Clear the tree.
243 
244  // Clear the frames category, except for the "All enabled" property, which is first.
246 
247  S_FrameInfo to_delete;
248  M_FrameInfo::iterator frame_it = frames_.begin();
249  M_FrameInfo::iterator frame_end = frames_.end();
250  for ( ; frame_it != frame_end; ++frame_it )
251  {
252  to_delete.insert( frame_it->second );
253  }
254 
255  S_FrameInfo::iterator delete_it = to_delete.begin();
256  S_FrameInfo::iterator delete_end = to_delete.end();
257  for ( ; delete_it != delete_end; ++delete_it )
258  {
259  deleteFrame( *delete_it, false );
260  }
261 
262  frames_.clear();
263 
264  update_timer_ = 0.0f;
265 
266  clearStatuses();
267 }
268 
270 {
271  root_node_->setVisible( true );
272 
273  names_node_->setVisible( show_names_property_->getBool() );
274  arrows_node_->setVisible( show_arrows_property_->getBool() );
275  axes_node_->setVisible( show_axes_property_->getBool() );
276 }
277 
279 {
280  root_node_->setVisible( false );
281  clear();
282 }
283 
285 {
286  names_node_->setVisible( show_names_property_->getBool() );
287 
288  M_FrameInfo::iterator it = frames_.begin();
289  M_FrameInfo::iterator end = frames_.end();
290  for (; it != end; ++it)
291  {
292  FrameInfo* frame = it->second;
293 
294  frame->updateVisibilityFromFrame();
295  }
296 }
297 
299 {
300  axes_node_->setVisible( show_axes_property_->getBool() );
301 
302  M_FrameInfo::iterator it = frames_.begin();
303  M_FrameInfo::iterator end = frames_.end();
304  for (; it != end; ++it)
305  {
306  FrameInfo* frame = it->second;
307 
308  frame->updateVisibilityFromFrame();
309  }
310 }
311 
313 {
314  arrows_node_->setVisible( show_arrows_property_->getBool() );
315 
316  M_FrameInfo::iterator it = frames_.begin();
317  M_FrameInfo::iterator end = frames_.end();
318  for (; it != end; ++it)
319  {
320  FrameInfo* frame = it->second;
321 
322  frame->updateVisibilityFromFrame();
323  }
324 }
325 
327 {
329  {
330  return;
331  }
332  bool enabled = all_enabled_property_->getBool();
333 
334  M_FrameInfo::iterator it = frames_.begin();
335  M_FrameInfo::iterator end = frames_.end();
336  for (; it != end; ++it)
337  {
338  FrameInfo* frame = it->second;
339 
340  frame->enabled_property_->setBool( enabled );
341  }
342 }
343 
344 void TFDisplay::update(float wall_dt, float ros_dt)
345 {
346  update_timer_ += wall_dt;
347  float update_rate = update_rate_property_->getFloat();
348  if( update_rate < 0.0001f || update_timer_ > update_rate )
349  {
350  updateFrames();
351 
352  update_timer_ = 0.0f;
353  }
354 }
355 
356 FrameInfo* TFDisplay::getFrameInfo( const std::string& frame )
357 {
358  M_FrameInfo::iterator it = frames_.find( frame );
359  if ( it == frames_.end() )
360  {
361  return NULL;
362  }
363 
364  return it->second;
365 }
366 
368 {
369  typedef std::vector<std::string> V_string;
370  V_string frames;
371  context_->getTFClient()->getFrameStrings( frames );
372  std::sort(frames.begin(), frames.end());
373 
374  S_FrameInfo current_frames;
375 
376  {
377  V_string::iterator it = frames.begin();
378  V_string::iterator end = frames.end();
379  for ( ; it != end; ++it )
380  {
381  const std::string& frame = *it;
382 
383  if ( frame.empty() )
384  {
385  continue;
386  }
387 
388  FrameInfo* info = getFrameInfo( frame );
389  if (!info)
390  {
391  info = createFrame(frame);
392  }
393  else
394  {
395  updateFrame(info);
396  }
397 
398  current_frames.insert( info );
399  }
400  }
401 
402  {
403  S_FrameInfo to_delete;
404  M_FrameInfo::iterator frame_it = frames_.begin();
405  M_FrameInfo::iterator frame_end = frames_.end();
406  for ( ; frame_it != frame_end; ++frame_it )
407  {
408  if ( current_frames.find( frame_it->second ) == current_frames.end() )
409  {
410  to_delete.insert( frame_it->second );
411  }
412  }
413 
414  S_FrameInfo::iterator delete_it = to_delete.begin();
415  S_FrameInfo::iterator delete_end = to_delete.end();
416  for ( ; delete_it != delete_end; ++delete_it )
417  {
418  deleteFrame( *delete_it, true );
419  }
420  }
421 
423 }
424 
425 static const Ogre::ColourValue ARROW_HEAD_COLOR(1.0f, 0.1f, 0.6f, 1.0f);
426 static const Ogre::ColourValue ARROW_SHAFT_COLOR(0.8f, 0.8f, 0.3f, 1.0f);
427 
428 FrameInfo* TFDisplay::createFrame(const std::string& frame)
429 {
430  FrameInfo* info = new FrameInfo( this );
431  frames_.insert( std::make_pair( frame, info ) );
432 
433  info->name_ = frame;
434  info->last_update_ = ros::Time::now();
435  info->axes_ = new Axes( scene_manager_, axes_node_, 0.2, 0.02 );
436  info->axes_->getSceneNode()->setVisible( show_axes_property_->getBool() );
437  info->selection_handler_.reset( new FrameSelectionHandler( info, this, context_ ));
438  info->selection_handler_->addTrackedObjects( info->axes_->getSceneNode() );
439 
440  info->name_text_ = new MovableText( frame, "Liberation Sans", 0.1 );
442  info->name_node_ = names_node_->createChildSceneNode();
443  info->name_node_->attachObject( info->name_text_ );
444  info->name_node_->setVisible( show_names_property_->getBool() );
445 
446  info->parent_arrow_ = new Arrow( scene_manager_, arrows_node_, 1.0f, 0.01, 1.0f, 0.08 );
447  info->parent_arrow_->getSceneNode()->setVisible( false );
448  info->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR);
449  info->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);
450 
451  info->enabled_property_ = new BoolProperty( QString::fromStdString( info->name_ ), true, "Enable or disable this individual frame.",
452  frames_category_, SLOT( updateVisibilityFromFrame() ), info );
453 
454  info->parent_property_ = new StringProperty( "Parent", "", "Parent of this frame. (Not editable)",
455  info->enabled_property_ );
456  info->parent_property_->setReadOnly( true );
457 
458  info->position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
459  "Position of this frame, in the current Fixed Frame. (Not editable)",
460  info->enabled_property_ );
461  info->position_property_->setReadOnly( true );
462 
463  info->orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
464  "Orientation of this frame, in the current Fixed Frame. (Not editable)",
465  info->enabled_property_ );
466  info->orientation_property_->setReadOnly( true );
467 
468  info->rel_position_property_ = new VectorProperty( "Relative Position", Ogre::Vector3::ZERO,
469  "Position of this frame, relative to it's parent frame. (Not editable)",
470  info->enabled_property_ );
471  info->rel_position_property_->setReadOnly( true );
472 
473  info->rel_orientation_property_ = new QuaternionProperty( "Relative Orientation", Ogre::Quaternion::IDENTITY,
474  "Orientation of this frame, relative to it's parent frame. (Not editable)",
475  info->enabled_property_ );
476  info->rel_orientation_property_->setReadOnly( true );
477 
478  // If the current frame was specified as disabled in the config file
479  // then its enabled state must be updated accordingly
480  if( frame_config_enabled_state_.count(frame) > 0 && !frame_config_enabled_state_[frame] )
481  {
482  info->enabled_property_->setBool(false);
483  }
484 
485  updateFrame( info );
486 
487  return info;
488 }
489 
490 Ogre::ColourValue lerpColor(const Ogre::ColourValue& start, const Ogre::ColourValue& end, float t)
491 {
492  return start * t + end * (1 - t);
493 }
494 
496 {
498 
499  // Check last received time so we can grey out/fade out frames that have stopped being published
500  ros::Time latest_time;
501  tf->getLatestCommonTime( fixed_frame_.toStdString(), frame->name_, latest_time, 0 );
502 
503  if(( latest_time != frame->last_time_to_fixed_ ) ||
504  ( latest_time == ros::Time() ))
505  {
506  frame->last_update_ = ros::Time::now();
507  frame->last_time_to_fixed_ = latest_time;
508  }
509 
510  // Fade from color -> grey, then grey -> fully transparent
511  ros::Duration age = ros::Time::now() - frame->last_update_;
512  float frame_timeout = frame_timeout_property_->getFloat();
513  float one_third_timeout = frame_timeout * 0.3333333f;
514  if( age > ros::Duration( frame_timeout ))
515  {
516  frame->parent_arrow_->getSceneNode()->setVisible(false);
517  frame->axes_->getSceneNode()->setVisible(false);
518  frame->name_node_->setVisible(false);
519  return;
520  }
521  else if (age > ros::Duration(one_third_timeout))
522  {
523  Ogre::ColourValue grey(0.7, 0.7, 0.7, 1.0);
524 
525  if (age > ros::Duration(one_third_timeout * 2))
526  {
527  float a = std::max(0.0, (frame_timeout - age.toSec())/one_third_timeout);
528  Ogre::ColourValue c = Ogre::ColourValue(grey.r, grey.g, grey.b, a);
529 
530  frame->axes_->setXColor(c);
531  frame->axes_->setYColor(c);
532  frame->axes_->setZColor(c);
533  frame->name_text_->setColor(c);
534  frame->parent_arrow_->setColor(c.r, c.g, c.b, c.a);
535  }
536  else
537  {
538  float t = std::max(0.0, (one_third_timeout * 2 - age.toSec())/one_third_timeout);
539  frame->axes_->setXColor(lerpColor(frame->axes_->getDefaultXColor(), grey, t));
540  frame->axes_->setYColor(lerpColor(frame->axes_->getDefaultYColor(), grey, t));
541  frame->axes_->setZColor(lerpColor(frame->axes_->getDefaultZColor(), grey, t));
542  frame->name_text_->setColor(lerpColor(Ogre::ColourValue::White, grey, t));
543  frame->parent_arrow_->setShaftColor(lerpColor(ARROW_SHAFT_COLOR, grey, t));
544  frame->parent_arrow_->setHeadColor(lerpColor(ARROW_HEAD_COLOR, grey, t));
545  }
546  }
547  else
548  {
549  frame->axes_->setToDefaultColors();
550  frame->name_text_->setColor(Ogre::ColourValue::White);
551  frame->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR);
552  frame->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);
553  }
554 
555  setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK");
556 
557  Ogre::Vector3 position;
558  Ogre::Quaternion orientation;
559  if( !context_->getFrameManager()->getTransform( frame->name_, ros::Time(), position, orientation ))
560  {
561  std::stringstream ss;
562  ss << "No transform from [" << frame->name_ << "] to frame [" << fixed_frame_.toStdString() << "]";
563  setStatusStd(StatusProperty::Warn, frame->name_, ss.str());
564  ROS_DEBUG( "Error transforming frame '%s' to frame '%s'", frame->name_.c_str(), qPrintable( fixed_frame_ ));
565  frame->name_node_->setVisible( false );
566  frame->axes_->getSceneNode()->setVisible( false );
567  frame->parent_arrow_->getSceneNode()->setVisible( false );
568  return;
569  }
570 
571  frame->selection_handler_->setPosition( position );
572  frame->selection_handler_->setOrientation( orientation );
573 
574  bool frame_enabled = frame->enabled_property_->getBool();
575 
576  frame->axes_->setPosition( position );
577  frame->axes_->setOrientation( orientation );
578  frame->axes_->getSceneNode()->setVisible( show_axes_property_->getBool() && frame_enabled);
579  float scale = scale_property_->getFloat();
580  frame->axes_->setScale( Ogre::Vector3( scale, scale, scale ));
581 
582  frame->name_node_->setPosition( position );
583  frame->name_node_->setVisible( show_names_property_->getBool() && frame_enabled );
584  frame->name_node_->setScale( scale, scale, scale );
585 
586  frame->position_property_->setVector( position );
587  frame->orientation_property_->setQuaternion( orientation );
588 
589  std::string old_parent = frame->parent_;
590  frame->parent_.clear();
591  bool has_parent = tf->getParent( frame->name_, ros::Time(), frame->parent_ );
592  if( has_parent )
593  {
594  // If this frame has no tree property or the parent has changed,
595  if( !frame->tree_property_ || old_parent != frame->parent_ )
596  {
597  // Look up the new parent.
598  M_FrameInfo::iterator parent_it = frames_.find( frame->parent_ );
599  if( parent_it != frames_.end() )
600  {
601  FrameInfo* parent = parent_it->second;
602 
603  // If the parent has a tree property, make a new tree property for this frame.
604  if( parent->tree_property_ )
605  {
606  if(!frame->tree_property_) {
607  frame->tree_property_ = new Property( QString::fromStdString( frame->name_ ), QVariant(), "", parent->tree_property_ );
608  } else {
609  frame->tree_property_->setParent(parent->tree_property_);
610  frame->tree_property_->setName(QString::fromStdString( frame->name_ ));
611  frame->tree_property_->setValue(QVariant());
612  frame->tree_property_->setDescription("");
613  }
614  }
615  }
616  }
617 
618  tf::StampedTransform transform;
619  try {
620  context_->getFrameManager()->getTFClientPtr()->lookupTransform(frame->parent_,frame->name_,ros::Time(0),transform);
621  }
622  catch(tf::TransformException& e)
623  {
624  ROS_DEBUG( "Error transforming frame '%s' (parent of '%s') to frame '%s'",
625  frame->parent_.c_str(), frame->name_.c_str(), qPrintable( fixed_frame_ ));
626  }
627 
628  // get the position/orientation relative to the parent frame
629  Ogre::Vector3 relative_position( transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z() );
630  Ogre::Quaternion relative_orientation( transform.getRotation().w(), transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z() );
631  frame->rel_position_property_->setVector( relative_position );
632  frame->rel_orientation_property_->setQuaternion( relative_orientation );
633 
635  {
636  Ogre::Vector3 parent_position;
637  Ogre::Quaternion parent_orientation;
638  if (!context_->getFrameManager()->getTransform(frame->parent_, ros::Time(), parent_position, parent_orientation))
639  {
640  ROS_DEBUG( "Error transforming frame '%s' (parent of '%s') to frame '%s'",
641  frame->parent_.c_str(), frame->name_.c_str(), qPrintable( fixed_frame_ ));
642  }
643 
644  Ogre::Vector3 direction = parent_position - position;
645  float distance = direction.length();
646  direction.normalise();
647 
648  Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
649 
650  frame->distance_to_parent_ = distance;
651  float head_length = ( distance < 0.1*scale ) ? (0.1*scale*distance) : 0.1*scale;
652  float shaft_length = distance - head_length;
653  // aleeper: This was changed from 0.02 and 0.08 to 0.01 and 0.04 to match proper radius handling in arrow.cpp
654  frame->parent_arrow_->set( shaft_length, 0.01*scale, head_length, 0.04*scale );
655 
656  if ( distance > 0.001f )
657  {
658  frame->parent_arrow_->getSceneNode()->setVisible( show_arrows_property_->getBool() && frame_enabled );
659  }
660  else
661  {
662  frame->parent_arrow_->getSceneNode()->setVisible( false );
663  }
664 
665  frame->parent_arrow_->setPosition( position );
666  frame->parent_arrow_->setOrientation( orient );
667  }
668  else
669  {
670  frame->parent_arrow_->getSceneNode()->setVisible( false );
671  }
672  }
673  else
674  {
675  if ( !frame->tree_property_ || old_parent != frame->parent_ )
676  {
677  if(!frame->tree_property_) {
678  frame->tree_property_ = new Property( QString::fromStdString( frame->name_ ), QVariant(), "", tree_category_ );
679  } else {
680  frame->tree_property_->setName(QString::fromStdString( frame->name_ ));
681  frame->tree_property_->setValue(QVariant());
682  frame->tree_property_->setDescription("");
684  }
685  }
686 
687  frame->parent_arrow_->getSceneNode()->setVisible( false );
688  }
689 
690  frame->parent_property_->setStdString( frame->parent_ );
691  frame->selection_handler_->setParentName( frame->parent_ );
692 }
693 
694 void TFDisplay::deleteFrame( FrameInfo* frame, bool delete_properties )
695 {
696  M_FrameInfo::iterator it = frames_.find( frame->name_ );
697  ROS_ASSERT( it != frames_.end() );
698 
699  frames_.erase( it );
700 
701  delete frame->axes_;
703  delete frame->parent_arrow_;
704  delete frame->name_text_;
705  scene_manager_->destroySceneNode( frame->name_node_->getName() );
706  if( delete_properties )
707  {
708  delete frame->enabled_property_;
709  delete frame->tree_property_;
710  }
711  delete frame;
712 }
713 
715 {
717 }
718 
720 {
721  Display::reset();
722  clear();
723 }
724 
726 // FrameInfo
727 
729  : display_( display )
730  , axes_( NULL )
731  , axes_coll_( 0 )
732  , parent_arrow_( NULL )
733  , name_text_( NULL )
734  , distance_to_parent_( 0.0f )
735  , arrow_orientation_(Ogre::Quaternion::IDENTITY)
736  , tree_property_( NULL )
737 {}
738 
740 {
741  bool enabled = enabled_property_->getBool();
742  selection_handler_->setEnabled( enabled );
743  setEnabled( enabled );
744 }
745 
747 {
748  bool enabled = selection_handler_->getEnabled();
749  enabled_property_->setBool( enabled );
750  setEnabled( enabled );
751 }
752 
753 void FrameInfo::setEnabled( bool enabled )
754 {
755  if( name_node_ )
756  {
757  name_node_->setVisible( display_->show_names_property_->getBool() && enabled );
758  }
759 
760  if( axes_ )
761  {
762  axes_->getSceneNode()->setVisible( display_->show_axes_property_->getBool() && enabled );
763  }
764 
765  if( parent_arrow_ )
766  {
767  if( distance_to_parent_ > 0.001f )
768  {
769  parent_arrow_->getSceneNode()->setVisible( display_->show_arrows_property_->getBool() && enabled );
770  }
771  else
772  {
773  parent_arrow_->getSceneNode()->setVisible( false );
774  }
775  }
776 
777  if( display_->all_enabled_property_->getBool() && !enabled)
778  {
782  }
783 
784  // Update the configuration that stores the enabled state of all frames
785  display_->frame_config_enabled_state_[this->name_] = enabled;
786 
788 }
789 
790 } // namespace rviz
791 
Ogre::ColourValue lerpColor(const Ogre::ColourValue &start, const Ogre::ColourValue &end, float t)
Definition: tf_display.cpp:490
Property * frames_category_
Definition: tf_display.h:122
#define NULL
Definition: global.h:37
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
Definition: display.h:157
FrameInfo(TFDisplay *display)
Definition: tf_display.cpp:728
void setMin(float min)
FloatProperty * scale_property_
Definition: tf_display.h:120
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
Definition: tf_display.cpp:344
bool changing_single_frame_enabled_state_
Definition: tf_display.h:125
void updateVisibilityFromSelection()
Update whether the frame is visible or not, based on the enabled_property_ in the selection handler...
Definition: tf_display.cpp:746
void getFrameStrings(std::vector< std::string > &ids) const
std::string name_
Definition: tf_display.h:148
BoolProperty * show_arrows_property_
Definition: tf_display.h:114
virtual void onEnable()
Derived classes override this to do the actual work of enabling themselves.
Definition: tf_display.cpp:269
virtual bool setVector(const Ogre::Vector3 &vector)
Ogre::SceneNode * root_node_
Definition: tf_display.h:100
f
virtual tf::TransformListener * getTFClient() const =0
Convenience function: returns getFrameManager()->getTFClient().
virtual void onDisable()
Derived classes override this to do the actual work of disabling themselves.
Definition: tf_display.cpp:278
std::set< FrameInfo * > S_FrameInfo
Definition: tf_display.cpp:159
StringProperty * parent_property_
Definition: tf_display.cpp:75
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
virtual void onInitialize()
Override this function to do subclass-specific initialization.
Definition: tf_display.cpp:207
bool getParent(const std::string &frame_id, ros::Time time, std::string &parent) const
void updateShowNames()
Definition: tf_display.cpp:284
Ogre::SceneNode * names_node_
Definition: tf_display.h:101
StringProperty * parent_property_
Definition: tf_display.h:167
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
Definition: property.cpp:130
Property * tree_category_
Definition: tf_display.h:123
virtual void createProperties(const Picked &obj, Property *parent_property)
Override to create properties of the given picked object(s).
Definition: tf_display.cpp:92
void setEnabled(bool enabled)
Definition: tf_display.cpp:127
int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time &time, std::string *error_string) const
Ogre::SceneNode * arrows_node_
Definition: tf_display.h:102
Displays a visual representation of the TF hierarchy.
Definition: tf_display.h:64
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
float distance_to_parent_
Definition: tf_display.h:157
virtual float getFloat() const
M_EnabledState frame_config_enabled_state_
Definition: tf_display.h:109
static const Ogre::ColourValue & getDefaultYColor()
Definition: axes.cpp:153
virtual void setName(const QString &name)
Set the name.
Definition: property.cpp:150
void removeObject(CollObjectHandle obj)
ros::Time last_update_
Definition: tf_display.h:160
void updateShowAxes()
Definition: tf_display.cpp:298
void allEnabledChanged()
Definition: tf_display.cpp:326
Internal class needed only by TFDisplay.
Definition: tf_display.h:130
void updateShowArrows()
Definition: tf_display.cpp:312
FrameSelectionHandler(FrameInfo *frame, TFDisplay *display, DisplayContext *context)
Definition: tf_display.cpp:80
bool isValid()
Return true if the iterator currently points to a valid entry, false if not.
Definition: config.cpp:385
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
virtual ~TFDisplay()
Definition: tf_display.cpp:198
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
void setZColor(const Ogre::ColourValue &col)
Definition: axes.cpp:132
static const Ogre::ColourValue & getDefaultXColor()
Definition: axes.cpp:148
Property specialized to enforce floating point max/min.
void set(float shaft_length, float shaft_diameter, float head_length, float head_diameter)
Set the parameters for this arrow.
Definition: arrow.cpp:71
BoolProperty * show_names_property_
Definition: tf_display.h:113
VectorProperty * rel_position_property_
Definition: tf_display.h:163
virtual void setDescription(const QString &description)
Set the description.
Definition: property.cpp:164
void setYColor(const Ogre::ColourValue &col)
Definition: axes.cpp:127
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:264
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this arrow.
Definition: arrow.h:143
static const Ogre::ColourValue ARROW_SHAFT_COLOR(0.8f, 0.8f, 0.3f, 1.0f)
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
Definition: axes.cpp:84
virtual void destroyProperties(const Picked &obj, Property *parent_property)
Destroy all properties for the given picked object(s).
Definition: tf_display.cpp:108
Configuration data storage class.
Definition: config.h:125
VectorProperty * position_property_
Definition: tf_display.h:165
virtual bool getBool() const
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
TFSIMD_FORCE_INLINE const tfScalar & x() const
void updateFrame(FrameInfo *frame)
Definition: tf_display.cpp:495
FrameInfo * getFrameInfo(const std::string &frame)
Definition: tf_display.cpp:356
QuaternionProperty * rel_orientation_property_
Definition: tf_display.h:164
void setXColor(const Ogre::ColourValue &col)
Definition: axes.cpp:122
static const Ogre::ColourValue ARROW_HEAD_COLOR(1.0f, 0.1f, 0.6f, 1.0f)
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of the base of the arrow.
Definition: arrow.cpp:111
virtual void removeChildren(int start_index=0, int count=-1)
Remove and delete some or all child Properties. Does not change the value of this Property...
Definition: property.cpp:100
Pure-virtual base class for objects which give Display subclasses context in which to work...
virtual void clearStatuses()
Delete all status children. This is thread-safe.
Definition: display.cpp:224
TFSIMD_FORCE_INLINE const tfScalar & z() const
void setParent(Property *new_parent)
Set parent property, without telling the parent.
Definition: property.cpp:231
CollObjectHandle axes_coll_
Definition: tf_display.h:151
std::string parent_
Definition: tf_display.h:149
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:300
const boost::shared_ptr< tf::TransformListener > & getTFClientPtr()
Return a boost shared pointer to the tf::TransformListener used to receive transform data...
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
Definition: tf_display.cpp:714
void setToDefaultColors()
Definition: axes.cpp:137
void updateVisibilityFromFrame()
Update whether the frame is visible or not, based on the enabled_property_ in this FrameInfo...
Definition: tf_display.cpp:739
BoolProperty * all_enabled_property_
Definition: tf_display.h:118
BoolProperty * enabled_property_
Definition: tf_display.cpp:74
QVariant getValue() const
If this config object is valid and is a Value type, this returns its value. Otherwise it returns an i...
Definition: config.cpp:312
Ogre::SceneNode * axes_node_
Definition: tf_display.h:103
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
Iterator class for looping over all entries in a Map type Config Node.
Definition: config.h:282
virtual void setColor(float r, float g, float b, float a)
Set the color of this arrow. Sets both the head and shaft color to the same value. Values are in the range [0, 1].
Definition: arrow.cpp:86
TFSIMD_FORCE_INLINE const tfScalar & y() const
virtual bool setQuaternion(const Ogre::Quaternion &quaternion)
void setEnabled(bool enabled)
Set this frame to be visible or invisible.
Definition: tf_display.cpp:753
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation.
Definition: arrow.cpp:116
virtual SelectionManager * getSelectionManager() const =0
Return a pointer to the SelectionManager.
BoolProperty * enabled_property_
Definition: tf_display.h:168
TFDisplay * display_
Definition: tf_display.h:147
void setOrientation(const Ogre::Quaternion &orientation)
Definition: tf_display.cpp:151
float update_timer_
Definition: tf_display.h:111
void setParentName(std::string parent_name)
Definition: tf_display.cpp:135
void setTextAlignment(const HorizontalAlignment &horizontalAlignment, const VerticalAlignment &verticalAlignment)
virtual void load(const Config &config)
Load the settings for this display from the given Config node, which must be a map.
Definition: tf_display.cpp:218
void setShaftColor(float r, float g, float b, float a=1.0f)
Set the color of the arrow&#39;s shaft. Values are in the range [0, 1].
Definition: arrow.cpp:101
Ogre::SceneNode * name_node_
Definition: tf_display.h:155
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
Definition: axes.h:58
std::vector< std::string > V_string
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
Definition: display.h:261
virtual void reset()
Called to tell the display to clear its state.
Definition: tf_display.cpp:719
FrameSelectionHandlerPtr selection_handler_
Definition: tf_display.h:152
FrameInfo * createFrame(const std::string &frame)
Definition: tf_display.cpp:428
VectorProperty * position_property_
Definition: tf_display.cpp:76
FloatProperty * update_rate_property_
Definition: tf_display.h:116
Property specialized for string values.
MovableText * name_text_
Definition: tf_display.h:154
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
virtual void load(const Config &config)
Load the settings for this display from the given Config node, which must be a map.
Definition: display.cpp:242
MapIterator mapIterator() const
Return a new iterator for looping over key/value pairs.
Definition: config.cpp:344
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
Definition: axes.cpp:94
BoolProperty * show_axes_property_
Definition: tf_display.h:115
void setColor(const Ogre::ColourValue &color)
Property(const QString &name=QString(), const QVariant default_value=QVariant(), const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
Constructor.
Definition: property.cpp:54
Quaternion getRotation() const
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
void setPosition(const Ogre::Vector3 &position)
Definition: tf_display.cpp:143
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
friend class FrameInfo
Definition: tf_display.h:126
An arrow consisting of a cylinder and a cone.
Definition: arrow.h:59
static Time now()
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Definition: axes.cpp:89
Config mapGetChild(const QString &key) const
If the referenced Node is a Map and it has a child with the given key, return a reference to the chil...
Definition: config.cpp:201
Arrow * parent_arrow_
Definition: tf_display.h:153
M_FrameInfo frames_
Definition: tf_display.h:106
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:247
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
Definition: axes.h:90
#define ROS_ASSERT(cond)
Property * tree_property_
Definition: tf_display.h:170
ros::Time last_time_to_fixed_
Definition: tf_display.h:161
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
Definition: property.h:397
void setHeadColor(float r, float g, float b, float a=1.0f)
Set the color of the arrow&#39;s head. Values are in the range [0, 1].
Definition: arrow.cpp:106
bool setStdString(const std::string &std_str)
FloatProperty * frame_timeout_property_
Definition: tf_display.h:117
bool setBool(bool value)
Definition: bool_property.h:62
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
static const Ogre::ColourValue & getDefaultZColor()
Definition: axes.cpp:158
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
QuaternionProperty * orientation_property_
Definition: tf_display.cpp:77
void deleteFrame(FrameInfo *frame, bool delete_properties)
Definition: tf_display.cpp:694
QuaternionProperty * orientation_property_
Definition: tf_display.h:166
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:42