30 #include <boost/bind.hpp> 32 #include <OgreSceneNode.h> 33 #include <OgreSceneManager.h> 163 , update_timer_( 0.0
f )
164 , changing_single_frame_enabled_state_( false )
178 "The interval, in seconds, at which to update the frame transforms. 0 means to do so every update cycle.",
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.",
192 "Whether all the frames should be enabled or not.",
195 tree_category_ =
new Property(
"Tree", QVariant(),
"A tree-view of the frames, showing the parent/child relationships.",
this );
228 QString key = iter.currentKey();
229 if( key !=
"All Enabled" )
231 const Config& child = iter.currentChild();
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 )
252 to_delete.insert( frame_it->second );
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 )
288 M_FrameInfo::iterator it =
frames_.begin();
289 M_FrameInfo::iterator end =
frames_.end();
290 for (; it != end; ++it)
302 M_FrameInfo::iterator it =
frames_.begin();
303 M_FrameInfo::iterator end =
frames_.end();
304 for (; it != end; ++it)
316 M_FrameInfo::iterator it =
frames_.begin();
317 M_FrameInfo::iterator end =
frames_.end();
318 for (; it != end; ++it)
334 M_FrameInfo::iterator it =
frames_.begin();
335 M_FrameInfo::iterator end =
frames_.end();
336 for (; it != end; ++it)
348 if( update_rate < 0.0001f || update_timer_ > update_rate )
358 M_FrameInfo::iterator it =
frames_.find( frame );
369 typedef std::vector<std::string>
V_string;
372 std::sort(frames.begin(), frames.end());
374 S_FrameInfo current_frames;
377 V_string::iterator it = frames.begin();
378 V_string::iterator end = frames.end();
379 for ( ; it != end; ++it )
381 const std::string& frame = *it;
398 current_frames.insert( info );
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 )
408 if ( current_frames.find( frame_it->second ) == current_frames.end() )
410 to_delete.insert( frame_it->second );
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 )
431 frames_.insert( std::make_pair( frame, info ) );
459 "Position of this frame, in the current Fixed Frame. (Not editable)",
464 "Orientation of this frame, in the current Fixed Frame. (Not editable)",
469 "Position of this frame, relative to it's parent frame. (Not editable)",
474 "Orientation of this frame, relative to it's parent frame. (Not editable)",
490 Ogre::ColourValue
lerpColor(
const Ogre::ColourValue& start,
const Ogre::ColourValue& end,
float t)
492 return start * t + end * (1 - t);
513 float one_third_timeout = frame_timeout * 0.3333333f;
523 Ogre::ColourValue grey(0.7, 0.7, 0.7, 1.0);
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);
538 float t = std::max(0.0, (one_third_timeout * 2 - age.
toSec())/one_third_timeout);
557 Ogre::Vector3 position;
558 Ogre::Quaternion orientation;
561 std::stringstream ss;
562 ss <<
"No transform from [" << frame->
name_ <<
"] to frame [" <<
fixed_frame_.toStdString() <<
"]";
580 frame->
axes_->
setScale( Ogre::Vector3( scale, scale, scale ));
584 frame->
name_node_->setScale( scale, scale, scale );
589 std::string old_parent = frame->
parent_;
599 if( parent_it !=
frames_.end() )
624 ROS_DEBUG(
"Error transforming frame '%s' (parent of '%s') to frame '%s'",
636 Ogre::Vector3 parent_position;
637 Ogre::Quaternion parent_orientation;
640 ROS_DEBUG(
"Error transforming frame '%s' (parent of '%s') to frame '%s'",
644 Ogre::Vector3 direction = parent_position - position;
645 float distance = direction.length();
646 direction.normalise();
648 Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction );
651 float head_length = ( distance < 0.1*scale ) ? (0.1*scale*distance) : 0.1*scale;
652 float shaft_length = distance - head_length;
654 frame->
parent_arrow_->
set( shaft_length, 0.01*scale, head_length, 0.04*scale );
656 if ( distance > 0.001
f )
696 M_FrameInfo::iterator it =
frames_.find( frame->
name_ );
706 if( delete_properties )
729 : display_( display )
732 , parent_arrow_(
NULL )
734 , distance_to_parent_( 0.0
f )
735 , arrow_orientation_(
Ogre::Quaternion::IDENTITY)
736 , tree_property_(
NULL )
Ogre::ColourValue lerpColor(const Ogre::ColourValue &start, const Ogre::ColourValue &end, float t)
Property * frames_category_
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...
FrameInfo(TFDisplay *display)
FloatProperty * scale_property_
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
bool changing_single_frame_enabled_state_
void updateVisibilityFromSelection()
Update whether the frame is visible or not, based on the enabled_property_ in the selection handler...
Property * category_property_
BoolProperty * show_arrows_property_
virtual void onEnable()
Derived classes override this to do the actual work of enabling themselves.
virtual bool setVector(const Ogre::Vector3 &vector)
Ogre::SceneNode * root_node_
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.
std::set< FrameInfo * > S_FrameInfo
StringProperty * parent_property_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
virtual void onInitialize()
Override this function to do subclass-specific initialization.
Ogre::SceneNode * names_node_
StringProperty * parent_property_
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...
Property * tree_category_
virtual void createProperties(const Picked &obj, Property *parent_property)
Override to create properties of the given picked object(s).
void setEnabled(bool enabled)
Ogre::SceneNode * arrows_node_
Displays a visual representation of the TF hierarchy.
A single element of a property tree, with a name, value, description, and possibly children...
float distance_to_parent_
virtual float getFloat() const
M_EnabledState frame_config_enabled_state_
static const Ogre::ColourValue & getDefaultYColor()
virtual void setName(const QString &name)
Set the name.
void removeObject(CollObjectHandle obj)
Internal class needed only by TFDisplay.
FrameSelectionHandler(FrameInfo *frame, TFDisplay *display, DisplayContext *context)
bool isValid()
Return true if the iterator currently points to a valid entry, false if not.
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
void setZColor(const Ogre::ColourValue &col)
static const Ogre::ColourValue & getDefaultXColor()
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.
BoolProperty * show_names_property_
VectorProperty * rel_position_property_
virtual void setDescription(const QString &description)
Set the description.
void setYColor(const Ogre::ColourValue &col)
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this arrow.
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.
virtual void destroyProperties(const Picked &obj, Property *parent_property)
Destroy all properties for the given picked object(s).
Configuration data storage class.
VectorProperty * position_property_
virtual bool getBool() const
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
TFSIMD_FORCE_INLINE const tfScalar & x() const
void updateFrame(FrameInfo *frame)
FrameInfo * getFrameInfo(const std::string &frame)
QuaternionProperty * rel_orientation_property_
void setXColor(const Ogre::ColourValue &col)
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.
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...
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.
TFSIMD_FORCE_INLINE const tfScalar & z() const
void setParent(Property *new_parent)
Set parent property, without telling the parent.
CollObjectHandle axes_coll_
virtual void reset()
Called to tell the display to clear its state.
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_.
void setToDefaultColors()
void updateVisibilityFromFrame()
Update whether the frame is visible or not, based on the enabled_property_ in this FrameInfo...
BoolProperty * all_enabled_property_
BoolProperty * enabled_property_
QVariant getValue() const
If this config object is valid and is a Value type, this returns its value. Otherwise it returns an i...
Ogre::SceneNode * axes_node_
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.
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].
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.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation.
virtual ~FrameSelectionHandler()
virtual SelectionManager * getSelectionManager() const =0
Return a pointer to the SelectionManager.
BoolProperty * enabled_property_
void setOrientation(const Ogre::Quaternion &orientation)
void setParentName(std::string parent_name)
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.
void setShaftColor(float r, float g, float b, float a=1.0f)
Set the color of the arrow's shaft. Values are in the range [0, 1].
Ogre::SceneNode * name_node_
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
std::vector< std::string > V_string
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
virtual void reset()
Called to tell the display to clear its state.
FrameSelectionHandlerPtr selection_handler_
FrameInfo * createFrame(const std::string &frame)
VectorProperty * position_property_
FloatProperty * update_rate_property_
Property specialized for string values.
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.
MapIterator mapIterator() const
Return a new iterator for looping over key/value pairs.
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
BoolProperty * show_axes_property_
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.
Property specialized to provide getter for booleans.
void setPosition(const Ogre::Vector3 &position)
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.
An arrow consisting of a cylinder and a cone.
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
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...
bool initialized() const
Returns true if the display has been initialized.
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
Property * tree_property_
ros::Time last_time_to_fixed_
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
void setHeadColor(float r, float g, float b, float a=1.0f)
Set the color of the arrow's head. Values are in the range [0, 1].
bool setStdString(const std::string &std_str)
FloatProperty * frame_timeout_property_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
static const Ogre::ColourValue & getDefaultZColor()
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_
void deleteFrame(FrameInfo *frame, bool delete_properties)
QuaternionProperty * orientation_property_