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 <OGRE/OgreSceneNode.h>
00031 #include <OGRE/OgreSceneManager.h>
00032 #include <OGRE/OgreManualObject.h>
00033 #include <OGRE/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 );
00051 alpha_property_ = new FloatProperty( "Alpha", 1.0,
00052 "Amount of transparency to apply to the polygon.", this );
00053 alpha_property_->setMin( 0 );
00054 alpha_property_->setMax( 1 );
00055 }
00056
00057 PolygonDisplay::~PolygonDisplay()
00058 {
00059 scene_manager_->destroyManualObject( manual_object_ );
00060 }
00061
00062 void PolygonDisplay::onInitialize()
00063 {
00064 MFDClass::onInitialize();
00065
00066 connect( color_property_, SIGNAL( changed() ), context_, SLOT( queueRender() ));
00067 connect( alpha_property_, SIGNAL( changed() ), context_, SLOT( queueRender() ));
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_DECLARE_CLASS( rviz, Polygon, rviz::PolygonDisplay, rviz::Display )