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 "axes_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/properties/property.h"
00033 #include "rviz/properties/property_manager.h"
00034 #include "rviz/frame_manager.h"
00035
00036 #include "rviz/ogre_helpers/axes.h"
00037
00038 #include <boost/bind.hpp>
00039
00040 #include <OGRE/OgreSceneNode.h>
00041 #include <OGRE/OgreSceneManager.h>
00042
00043 namespace rviz
00044 {
00045
00046 AxesDisplay::AxesDisplay()
00047 : Display()
00048 , length_( 1.0 )
00049 , radius_( 0.1 )
00050 , axes_( 0 )
00051 {
00052 }
00053
00054 AxesDisplay::~AxesDisplay()
00055 {
00056 delete axes_;
00057 }
00058
00059 void AxesDisplay::onInitialize()
00060 {
00061 axes_ = new Axes( scene_manager_, 0, length_, radius_ );
00062
00063 axes_->getSceneNode()->setVisible( isEnabled() );
00064
00065 setFrame(FIXED_FRAME_STRING);
00066 }
00067
00068 void AxesDisplay::onEnable()
00069 {
00070 axes_->getSceneNode()->setVisible( true );
00071 }
00072
00073 void AxesDisplay::onDisable()
00074 {
00075 axes_->getSceneNode()->setVisible( false );
00076 }
00077
00078 void AxesDisplay::create()
00079 {
00080 axes_->set( length_, radius_ );
00081
00082 causeRender();
00083 }
00084
00085 void AxesDisplay::set( float length, float radius )
00086 {
00087 length_ = length;
00088 radius_ = radius;
00089
00090 create();
00091
00092 propertyChanged(length_property_);
00093 propertyChanged(radius_property_);
00094 }
00095
00096 void AxesDisplay::setFrame(const std::string& frame)
00097 {
00098 frame_ = frame;
00099 propertyChanged(frame_property_);
00100 }
00101
00102 void AxesDisplay::setLength( float length )
00103 {
00104 set( length, radius_ );
00105 }
00106
00107 void AxesDisplay::setRadius( float radius )
00108 {
00109 set( length_, radius );
00110 }
00111
00112 void AxesDisplay::update(float dt, float ros_dt)
00113 {
00114 std::string frame = frame_;
00115 if (frame == FIXED_FRAME_STRING)
00116 {
00117 frame = fixed_frame_;
00118 }
00119
00120 Ogre::Vector3 position;
00121 Ogre::Quaternion orientation;
00122 if (vis_manager_->getFrameManager()->getTransform(frame, ros::Time(), position, orientation))
00123 {
00124 axes_->setPosition(position);
00125 axes_->setOrientation(orientation);
00126 setStatus(status_levels::Ok, "Transform", "Transform OK");
00127 }
00128 else
00129 {
00130 std::string error;
00131 if (vis_manager_->getFrameManager()->transformHasProblems(frame, ros::Time(), error))
00132 {
00133 setStatus(status_levels::Error, "Transform", error);
00134 }
00135 else
00136 {
00137 std::stringstream ss;
00138 ss << "Could not transform from [" << frame << "] to Fixed Frame [" << fixed_frame_ << "] for an unknown reason";
00139 setStatus(status_levels::Error, "Transform", ss.str());
00140 }
00141 }
00142 }
00143
00144 void AxesDisplay::createProperties()
00145 {
00146 frame_property_ = property_manager_->createProperty<TFFrameProperty>("Reference Frame", property_prefix_, boost::bind(&AxesDisplay::getFrame, this),
00147 boost::bind(&AxesDisplay::setFrame, this, _1), parent_category_, this);
00148 setPropertyHelpText(frame_property_, "The TF frame these axes will use for their origin.");
00149 length_property_ = property_manager_->createProperty<FloatProperty>( "Length", property_prefix_, boost::bind( &AxesDisplay::getLength, this ),
00150 boost::bind( &AxesDisplay::setLength, this, _1 ), parent_category_, this );
00151 FloatPropertyPtr float_prop = length_property_.lock();
00152 float_prop->setMin( 0.0001 );
00153 setPropertyHelpText(length_property_, "Length of each axis, in meters.");
00154
00155 radius_property_ = property_manager_->createProperty<FloatProperty>( "Radius", property_prefix_, boost::bind( &AxesDisplay::getRadius, this ),
00156 boost::bind( &AxesDisplay::setRadius, this, _1 ), parent_category_, this );
00157 float_prop = radius_property_.lock();
00158 float_prop->setMin( 0.0001 );
00159 setPropertyHelpText(radius_property_, "Width of each axis, in meters.");
00160 }
00161
00162 }