00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <boost/bind.hpp>
00031
00032 #include <OGRE/OgreSceneNode.h>
00033 #include <OGRE/OgreSceneManager.h>
00034
00035 #include "rviz/display_context.h"
00036 #include "rviz/frame_manager.h"
00037 #include "rviz/ogre_helpers/axes.h"
00038 #include "rviz/properties/float_property.h"
00039 #include "rviz/properties/tf_frame_property.h"
00040
00041 #include "axes_display.h"
00042
00043 namespace rviz
00044 {
00045
00046 AxesDisplay::AxesDisplay()
00047 : Display()
00048 , axes_( 0 )
00049 {
00050 frame_property_ = new TfFrameProperty( "Reference Frame", TfFrameProperty::FIXED_FRAME_STRING,
00051 "The TF frame these axes will use for their origin.",
00052 this, NULL, true );
00053
00054 length_property_ = new FloatProperty( "Length", 1.0,
00055 "Length of each axis, in meters.",
00056 this, SLOT( updateShape() ));
00057 length_property_->setMin( 0.0001 );
00058
00059 radius_property_ = new FloatProperty( "Radius", 0.1,
00060 "Radius of each axis, in meters.",
00061 this, SLOT( updateShape() ));
00062 radius_property_->setMin( 0.0001 );
00063 }
00064
00065 AxesDisplay::~AxesDisplay()
00066 {
00067 delete axes_;
00068 }
00069
00070 void AxesDisplay::onInitialize()
00071 {
00072 frame_property_->setFrameManager( context_->getFrameManager() );
00073
00074 axes_ = new Axes( scene_manager_, 0, length_property_->getFloat(), radius_property_->getFloat() );
00075 axes_->getSceneNode()->setVisible( isEnabled() );
00076 }
00077
00078 void AxesDisplay::onEnable()
00079 {
00080 axes_->getSceneNode()->setVisible( true );
00081 }
00082
00083 void AxesDisplay::onDisable()
00084 {
00085 axes_->getSceneNode()->setVisible( false );
00086 }
00087
00088 void AxesDisplay::updateShape()
00089 {
00090 axes_->set( length_property_->getFloat(), radius_property_->getFloat() );
00091 context_->queueRender();
00092 }
00093
00094 void AxesDisplay::update( float dt, float ros_dt )
00095 {
00096 QString qframe = frame_property_->getFrame();
00097 std::string frame = qframe.toStdString();
00098
00099 Ogre::Vector3 position;
00100 Ogre::Quaternion orientation;
00101 if( context_->getFrameManager()->getTransform( frame, ros::Time(), position, orientation ))
00102 {
00103 axes_->setPosition( position );
00104 axes_->setOrientation( orientation );
00105 setStatus( StatusProperty::Ok, "Transform", "Transform OK" );
00106 }
00107 else
00108 {
00109 std::string error;
00110 if( context_->getFrameManager()->transformHasProblems( frame, ros::Time(), error ))
00111 {
00112 setStatus( StatusProperty::Error, "Transform", QString::fromStdString( error ));
00113 }
00114 else
00115 {
00116 setStatus( StatusProperty::Error,
00117 "Transform",
00118 "Could not transform from [" + qframe + "] to Fixed Frame [" + fixed_frame_ + "] for an unknown reason" );
00119 }
00120 }
00121 }
00122
00123 }
00124
00125 #include <pluginlib/class_list_macros.h>
00126 PLUGINLIB_DECLARE_CLASS( rviz, Axes, rviz::AxesDisplay, rviz::Display )