Go to the documentation of this file.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 <OgreManualObject.h>
00031 #include <OgreSceneManager.h>
00032 #include <OgreSceneNode.h>
00033
00034 #include "rviz/display_context.h"
00035 #include "rviz/frame_manager.h"
00036 #include "rviz/properties/color_property.h"
00037 #include "rviz/properties/float_property.h"
00038 #include "rviz/validate_floats.h"
00039
00040 #include "rviz/default_plugin/pose_array_display.h"
00041
00042 namespace rviz
00043 {
00044
00045 PoseArrayDisplay::PoseArrayDisplay()
00046 : manual_object_( NULL )
00047 {
00048 color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrows.", this );
00049 length_property_ = new FloatProperty( "Arrow Length", 0.3, "Length of the arrows.", this );
00050 }
00051
00052 PoseArrayDisplay::~PoseArrayDisplay()
00053 {
00054 if ( initialized() )
00055 {
00056 scene_manager_->destroyManualObject( manual_object_ );
00057 }
00058 }
00059
00060 void PoseArrayDisplay::onInitialize()
00061 {
00062 MFDClass::onInitialize();
00063 manual_object_ = scene_manager_->createManualObject();
00064 manual_object_->setDynamic( true );
00065 scene_node_->attachObject( manual_object_ );
00066 }
00067
00068 bool validateFloats( const geometry_msgs::PoseArray& msg )
00069 {
00070 return validateFloats( msg.poses );
00071 }
00072
00073 void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr& msg )
00074 {
00075 if( !validateFloats( *msg ))
00076 {
00077 setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00078 return;
00079 }
00080
00081 manual_object_->clear();
00082
00083 Ogre::Vector3 position;
00084 Ogre::Quaternion orientation;
00085 if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00086 {
00087 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00088 }
00089
00090 scene_node_->setPosition( position );
00091 scene_node_->setOrientation( orientation );
00092
00093 manual_object_->clear();
00094
00095 Ogre::ColourValue color = color_property_->getOgreColor();
00096 float length = length_property_->getFloat();
00097 size_t num_poses = msg->poses.size();
00098 manual_object_->estimateVertexCount( num_poses * 6 );
00099 manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
00100 for( size_t i=0; i < num_poses; ++i )
00101 {
00102 Ogre::Vector3 pos( msg->poses[i].position.x,
00103 msg->poses[i].position.y,
00104 msg->poses[i].position.z );
00105 Ogre::Quaternion orient( msg->poses[i].orientation.w,
00106 msg->poses[i].orientation.x,
00107 msg->poses[i].orientation.y,
00108 msg->poses[i].orientation.z );
00109
00110
00111
00112 Ogre::Vector3 vertices[6];
00113 vertices[0] = pos;
00114 vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 );
00115 vertices[2] = vertices[ 1 ];
00116 vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 );
00117 vertices[4] = vertices[ 1 ];
00118 vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 );
00119
00120 for( int i = 0; i < 6; ++i )
00121 {
00122 manual_object_->position( vertices[i] );
00123 manual_object_->colour( color );
00124 }
00125 }
00126 manual_object_->end();
00127
00128 context_->queueRender();
00129 }
00130
00131 void PoseArrayDisplay::reset()
00132 {
00133 MFDClass::reset();
00134 if( manual_object_ )
00135 {
00136 manual_object_->clear();
00137 }
00138 }
00139
00140 }
00141
00142 #include <pluginlib/class_list_macros.h>
00143 PLUGINLIB_EXPORT_CLASS( rviz::PoseArrayDisplay, rviz::Display )