pose_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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     // properties_.size() should only be > 0 after createProperties()
00096     // and before destroyProperties(), during which frame_property_,
00097     // position_property_, and orientation_property_ should be valid
00098     // pointers.
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   // aleeper: default changed from 0.1 to match change in arrow.cpp
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   // aleeper: default changed from 0.2 to match change in arrow.cpp
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   // Arrow points in -Z direction, so rotate the orientation before display.
00166   // TODO: is it safe to change Arrow to point in +X direction?
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 } // namespace rviz
00292 
00293 #include <pluginlib/class_list_macros.h>
00294 PLUGINLIB_EXPORT_CLASS( rviz::PoseDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:15