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 <OgreSceneNode.h>
00031 #include <OgreSceneManager.h>
00032 #include <OgreManualObject.h>
00033 #include <OgreBillboardSet.h>
00034
00035 #include "rviz/display_context.h"
00036 #include "rviz/frame_manager.h"
00037 #include "rviz/properties/color_property.h"
00038 #include "rviz/properties/float_property.h"
00039 #include "rviz/properties/parse_color.h"
00040 #include "rviz/validate_floats.h"
00041
00042 #include "polygon_display.h"
00043
00044 namespace rviz
00045 {
00046
00047 PolygonDisplay::PolygonDisplay()
00048 {
00049 color_property_ = new ColorProperty( "Color", QColor( 25, 255, 0 ),
00050 "Color to draw the polygon.", this, SLOT( queueRender() ));
00051 alpha_property_ = new FloatProperty( "Alpha", 1.0,
00052 "Amount of transparency to apply to the polygon.", this, SLOT( queueRender() ));
00053 alpha_property_->setMin( 0 );
00054 alpha_property_->setMax( 1 );
00055 }
00056
00057 PolygonDisplay::~PolygonDisplay()
00058 {
00059 if ( initialized() )
00060 {
00061 scene_manager_->destroyManualObject( manual_object_ );
00062 }
00063 }
00064
00065 void PolygonDisplay::onInitialize()
00066 {
00067 MFDClass::onInitialize();
00068
00069 manual_object_ = scene_manager_->createManualObject();
00070 manual_object_->setDynamic( true );
00071 scene_node_->attachObject( manual_object_ );
00072 }
00073
00074 void PolygonDisplay::reset()
00075 {
00076 MFDClass::reset();
00077 manual_object_->clear();
00078 }
00079
00080 bool validateFloats( const geometry_msgs::PolygonStamped& msg )
00081 {
00082 return validateFloats(msg.polygon.points);
00083 }
00084
00085 void PolygonDisplay::processMessage(const geometry_msgs::PolygonStamped::ConstPtr& msg)
00086 {
00087 if( !validateFloats( *msg ))
00088 {
00089 setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00090 return;
00091 }
00092
00093 Ogre::Vector3 position;
00094 Ogre::Quaternion orientation;
00095 if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00096 {
00097 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00098 msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00099 }
00100
00101 scene_node_->setPosition( position );
00102 scene_node_->setOrientation( orientation );
00103
00104 manual_object_->clear();
00105
00106 Ogre::ColourValue color = qtToOgre( color_property_->getColor() );
00107 color.a = alpha_property_->getFloat();
00108
00109
00110
00111
00112
00113 uint32_t num_points = msg->polygon.points.size();
00114 if( num_points > 0 )
00115 {
00116 manual_object_->estimateVertexCount( num_points );
00117 manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
00118 for( uint32_t i=0; i < num_points + 1; ++i )
00119 {
00120 const geometry_msgs::Point32& msg_point = msg->polygon.points[ i % num_points ];
00121 manual_object_->position( msg_point.x, msg_point.y, msg_point.z );
00122 manual_object_->colour( color );
00123 }
00124
00125 manual_object_->end();
00126 }
00127 }
00128
00129 }
00130
00131 #include <pluginlib/class_list_macros.h>
00132 PLUGINLIB_EXPORT_CLASS( rviz::PolygonDisplay, rviz::Display )