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 <OgreEntity.h>
00031 #include <OgreSceneNode.h>
00032
00033 #include "rviz/display_context.h"
00034 #include "rviz/frame_manager.h"
00035 #include "rviz/ogre_helpers/arrow.h"
00036 #include "rviz/ogre_helpers/axes.h"
00037 #include "rviz/ogre_helpers/shape.h"
00038 #include "rviz/properties/color_property.h"
00039 #include "rviz/properties/enum_property.h"
00040 #include "rviz/properties/float_property.h"
00041 #include "rviz/properties/quaternion_property.h"
00042 #include "rviz/properties/string_property.h"
00043 #include "rviz/properties/vector_property.h"
00044 #include "rviz/selection/selection_manager.h"
00045 #include "rviz/validate_floats.h"
00046
00047 #include "rviz/default_plugin/pose_display.h"
00048
00049 namespace rviz
00050 {
00051
00052 class PoseDisplaySelectionHandler: public SelectionHandler
00053 {
00054 public:
00055 PoseDisplaySelectionHandler( PoseDisplay* display, DisplayContext* context )
00056 : SelectionHandler( context )
00057 , display_( display )
00058 {}
00059
00060 void createProperties( const Picked& obj, Property* parent_property )
00061 {
00062 Property* cat = new Property( "Pose " + display_->getName(), QVariant(), "", parent_property );
00063 properties_.push_back( cat );
00064
00065 frame_property_ = new StringProperty( "Frame", "", "", cat );
00066 frame_property_->setReadOnly( true );
00067
00068 position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, "", cat );
00069 position_property_->setReadOnly( true );
00070
00071 orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, "", cat );
00072 orientation_property_->setReadOnly( true );
00073 }
00074
00075 void getAABBs( const Picked& obj, V_AABB& aabbs )
00076 {
00077 if( display_->pose_valid_ )
00078 {
00079 if( display_->shape_property_->getOptionInt() == PoseDisplay::Arrow )
00080 {
00081 aabbs.push_back( display_->arrow_->getHead()->getEntity()->getWorldBoundingBox() );
00082 aabbs.push_back( display_->arrow_->getShaft()->getEntity()->getWorldBoundingBox() );
00083 }
00084 else
00085 {
00086 aabbs.push_back( display_->axes_->getXShape()->getEntity()->getWorldBoundingBox() );
00087 aabbs.push_back( display_->axes_->getYShape()->getEntity()->getWorldBoundingBox() );
00088 aabbs.push_back( display_->axes_->getZShape()->getEntity()->getWorldBoundingBox() );
00089 }
00090 }
00091 }
00092
00093 void setMessage(const geometry_msgs::PoseStampedConstPtr& message)
00094 {
00095
00096
00097
00098
00099 if( properties_.size() > 0 )
00100 {
00101 frame_property_->setStdString( message->header.frame_id );
00102 position_property_->setVector( Ogre::Vector3( message->pose.position.x,
00103 message->pose.position.y,
00104 message->pose.position.z ));
00105 orientation_property_->setQuaternion( Ogre::Quaternion( message->pose.orientation.w,
00106 message->pose.orientation.x,
00107 message->pose.orientation.y,
00108 message->pose.orientation.z ));
00109 }
00110 }
00111
00112 private:
00113 PoseDisplay* display_;
00114 StringProperty* frame_property_;
00115 VectorProperty* position_property_;
00116 QuaternionProperty* orientation_property_;
00117 };
00118
00119 PoseDisplay::PoseDisplay()
00120 : pose_valid_( false )
00121 {
00122 shape_property_ = new EnumProperty( "Shape", "Arrow", "Shape to display the pose as.",
00123 this, SLOT( updateShapeChoice() ));
00124 shape_property_->addOption( "Arrow", Arrow );
00125 shape_property_->addOption( "Axes", Axes );
00126
00127 color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrow.",
00128 this, SLOT( updateColorAndAlpha() ));
00129
00130 alpha_property_ = new FloatProperty( "Alpha", 1, "Amount of transparency to apply to the arrow.",
00131 this, SLOT( updateColorAndAlpha() ));
00132 alpha_property_->setMin( 0 );
00133 alpha_property_->setMax( 1 );
00134
00135 shaft_length_property_ = new FloatProperty( "Shaft Length", 1, "Length of the arrow's shaft, in meters.",
00136 this, SLOT( updateArrowGeometry() ));
00137
00138
00139 shaft_radius_property_ = new FloatProperty( "Shaft Radius", 0.05, "Radius of the arrow's shaft, in meters.",
00140 this, SLOT( updateArrowGeometry() ));
00141
00142 head_length_property_ = new FloatProperty( "Head Length", 0.3, "Length of the arrow's head, in meters.",
00143 this, SLOT( updateArrowGeometry() ));
00144
00145
00146 head_radius_property_ = new FloatProperty( "Head Radius", 0.1, "Radius of the arrow's head, in meters.",
00147 this, SLOT( updateArrowGeometry() ));
00148
00149 axes_length_property_ = new FloatProperty( "Axes Length", 1, "Length of each axis, in meters.",
00150 this, SLOT( updateAxisGeometry() ));
00151
00152 axes_radius_property_ = new FloatProperty( "Axes Radius", 0.1, "Radius of each axis, in meters.",
00153 this, SLOT( updateAxisGeometry() ));
00154 }
00155
00156 void PoseDisplay::onInitialize()
00157 {
00158 MFDClass::onInitialize();
00159
00160 arrow_ = new rviz::Arrow( scene_manager_, scene_node_,
00161 shaft_length_property_->getFloat(),
00162 shaft_radius_property_->getFloat(),
00163 head_length_property_->getFloat(),
00164 head_radius_property_->getFloat() );
00165
00166
00167 arrow_->setOrientation( Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ));
00168
00169 axes_ = new rviz::Axes( scene_manager_, scene_node_,
00170 axes_length_property_->getFloat(),
00171 axes_radius_property_->getFloat() );
00172
00173 updateShapeChoice();
00174 updateColorAndAlpha();
00175
00176 coll_handler_.reset( new PoseDisplaySelectionHandler( this, context_ ));
00177 coll_handler_->addTrackedObjects( arrow_->getSceneNode() );
00178 coll_handler_->addTrackedObjects( axes_->getSceneNode() );
00179 }
00180
00181 PoseDisplay::~PoseDisplay()
00182 {
00183 if ( initialized() )
00184 {
00185 delete arrow_;
00186 delete axes_;
00187 }
00188 }
00189
00190 void PoseDisplay::onEnable()
00191 {
00192 MFDClass::onEnable();
00193 updateShapeVisibility();
00194 }
00195
00196 void PoseDisplay::updateColorAndAlpha()
00197 {
00198 Ogre::ColourValue color = color_property_->getOgreColor();
00199 color.a = alpha_property_->getFloat();
00200
00201 arrow_->setColor( color );
00202
00203 context_->queueRender();
00204 }
00205
00206 void PoseDisplay::updateArrowGeometry()
00207 {
00208 arrow_->set( shaft_length_property_->getFloat(),
00209 shaft_radius_property_->getFloat(),
00210 head_length_property_->getFloat(),
00211 head_radius_property_->getFloat() );
00212 context_->queueRender();
00213 }
00214
00215 void PoseDisplay::updateAxisGeometry()
00216 {
00217 axes_->set( axes_length_property_->getFloat(),
00218 axes_radius_property_->getFloat() );
00219 context_->queueRender();
00220 }
00221
00222 void PoseDisplay::updateShapeChoice()
00223 {
00224 bool use_arrow = ( shape_property_->getOptionInt() == Arrow );
00225
00226 color_property_->setHidden( !use_arrow );
00227 alpha_property_->setHidden( !use_arrow );
00228 shaft_length_property_->setHidden( !use_arrow );
00229 shaft_radius_property_->setHidden( !use_arrow );
00230 head_length_property_->setHidden( !use_arrow );
00231 head_radius_property_->setHidden( !use_arrow );
00232
00233 axes_length_property_->setHidden( use_arrow );
00234 axes_radius_property_->setHidden( use_arrow );
00235
00236 updateShapeVisibility();
00237
00238 context_->queueRender();
00239 }
00240
00241 void PoseDisplay::updateShapeVisibility()
00242 {
00243 if( !pose_valid_ )
00244 {
00245 arrow_->getSceneNode()->setVisible( false );
00246 axes_->getSceneNode()->setVisible( false );
00247 }
00248 else
00249 {
00250 bool use_arrow = (shape_property_->getOptionInt() == Arrow);
00251 arrow_->getSceneNode()->setVisible( use_arrow );
00252 axes_->getSceneNode()->setVisible( !use_arrow );
00253 }
00254 }
00255
00256 void PoseDisplay::processMessage( const geometry_msgs::PoseStamped::ConstPtr& message )
00257 {
00258 if( !validateFloats( *message ))
00259 {
00260 setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00261 return;
00262 }
00263
00264 Ogre::Vector3 position;
00265 Ogre::Quaternion orientation;
00266 if( !context_->getFrameManager()->transform( message->header, message->pose, position, orientation ))
00267 {
00268 ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
00269 qPrintable( getName() ), message->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00270 return;
00271 }
00272
00273 pose_valid_ = true;
00274 updateShapeVisibility();
00275
00276 scene_node_->setPosition( position );
00277 scene_node_->setOrientation( orientation );
00278
00279 coll_handler_->setMessage( message );
00280
00281 context_->queueRender();
00282 }
00283
00284 void PoseDisplay::reset()
00285 {
00286 MFDClass::reset();
00287 pose_valid_ = false;
00288 updateShapeVisibility();
00289 }
00290
00291 }
00292
00293 #include <pluginlib/class_list_macros.h>
00294 PLUGINLIB_EXPORT_CLASS( rviz::PoseDisplay, rviz::Display )