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 <stdint.h>
00031
00032 #include <boost/bind.hpp>
00033
00034 #include <OgreSceneNode.h>
00035 #include <OgreSceneManager.h>
00036
00037 #include "rviz/display_context.h"
00038 #include "rviz/frame_manager.h"
00039 #include "rviz/ogre_helpers/grid.h"
00040 #include "rviz/properties/parse_color.h"
00041 #include "rviz/properties/property.h"
00042 #include "rviz/selection/selection_manager.h"
00043
00044 #include "grid_display.h"
00045
00046 namespace rviz
00047 {
00048
00049 GridDisplay::GridDisplay()
00050 : Display()
00051 {
00052 frame_property_ = new TfFrameProperty( "Reference Frame", TfFrameProperty::FIXED_FRAME_STRING,
00053 "The TF frame this grid will use for its origin.",
00054 this, 0, true );
00055
00056 cell_count_property_ = new IntProperty( "Plane Cell Count", 10,
00057 "The number of cells to draw in the plane of the grid.",
00058 this, SLOT( updateCellCount() ));
00059 cell_count_property_->setMin( 1 );
00060
00061 height_property_ = new IntProperty( "Normal Cell Count", 0,
00062 "The number of cells to draw along the normal vector of the grid. "
00063 " Setting to anything but 0 makes the grid 3D.",
00064 this, SLOT( updateHeight() ));
00065 height_property_->setMin( 0 );
00066
00067 cell_size_property_ = new FloatProperty( "Cell Size", 1.0f,
00068 "The length, in meters, of the side of each cell.",
00069 this, SLOT( updateCellSize() ));
00070 cell_size_property_->setMin( 0.0001 );
00071
00072 style_property_ = new EnumProperty( "Line Style", "Lines",
00073 "The rendering operation to use to draw the grid lines.",
00074 this, SLOT( updateStyle() ));
00075 style_property_->addOption( "Lines", Grid::Lines );
00076 style_property_->addOption( "Billboards", Grid::Billboards );
00077
00078 line_width_property_ = new FloatProperty( "Line Width", 0.03,
00079 "The width, in meters, of each grid line.",
00080 style_property_, SLOT( updateLineWidth() ), this );
00081 line_width_property_->setMin( 0.001 );
00082 line_width_property_->hide();
00083
00084 color_property_ = new ColorProperty( "Color", Qt::gray,
00085 "The color of the grid lines.",
00086 this, SLOT( updateColor() ));
00087 alpha_property_ = new FloatProperty( "Alpha", 0.5f,
00088 "The amount of transparency to apply to the grid lines.",
00089 this, SLOT( updateColor() ));
00090 alpha_property_->setMin( 0.0f );
00091 alpha_property_->setMax( 1.0f );
00092
00093 plane_property_ = new EnumProperty( "Plane", "XY",
00094 "The plane to draw the grid along.",
00095 this, SLOT( updatePlane() ));
00096 plane_property_->addOption( "XY", XY );
00097 plane_property_->addOption( "XZ", XZ );
00098 plane_property_->addOption( "YZ", YZ );
00099
00100 offset_property_ = new VectorProperty( "Offset", Ogre::Vector3::ZERO,
00101 "Allows you to offset the grid from the origin of the reference frame. In meters.",
00102 this, SLOT( updateOffset() ));
00103 }
00104
00105 GridDisplay::~GridDisplay()
00106 {
00107 if ( initialized() )
00108 {
00109 delete grid_;
00110 }
00111 }
00112
00113 void GridDisplay::onInitialize()
00114 {
00115 QColor color = color_property_->getColor();
00116 color.setAlphaF( alpha_property_->getFloat() );
00117
00118 frame_property_->setFrameManager( context_->getFrameManager() );
00119 grid_ = new Grid( scene_manager_, scene_node_,
00120 (Grid::Style) style_property_->getOptionInt(),
00121 cell_count_property_->getInt(),
00122 cell_size_property_->getFloat(),
00123 line_width_property_->getFloat(),
00124 qtToOgre( color ));
00125
00126 grid_->getSceneNode()->setVisible( false );
00127 updatePlane();
00128 }
00129
00130 void GridDisplay::update(float dt, float ros_dt)
00131 {
00132 QString qframe = frame_property_->getFrame();
00133 std::string frame = qframe.toStdString();
00134
00135 Ogre::Vector3 position;
00136 Ogre::Quaternion orientation;
00137 if( context_->getFrameManager()->getTransform( frame, ros::Time(), position, orientation ))
00138 {
00139 scene_node_->setPosition( position );
00140 scene_node_->setOrientation( orientation );
00141 setStatus( StatusProperty::Ok, "Transform", "Transform OK" );
00142 }
00143 else
00144 {
00145 std::string error;
00146 if( context_->getFrameManager()->transformHasProblems( frame, ros::Time(), error ))
00147 {
00148 setStatus( StatusProperty::Error, "Transform", QString::fromStdString( error ));
00149 }
00150 else
00151 {
00152 setStatus( StatusProperty::Error, "Transform",
00153 "Could not transform from [" + qframe + "] to [" + fixed_frame_ + "]" );
00154 }
00155 }
00156 }
00157
00158 void GridDisplay::updateColor()
00159 {
00160 QColor color = color_property_->getColor();
00161 color.setAlphaF( alpha_property_->getFloat() );
00162 grid_->setColor( qtToOgre( color ));
00163 context_->queueRender();
00164 }
00165
00166 void GridDisplay::updateCellSize()
00167 {
00168 grid_->setCellLength( cell_size_property_->getFloat() );
00169 context_->queueRender();
00170 }
00171
00172 void GridDisplay::updateCellCount()
00173 {
00174 grid_->setCellCount( cell_count_property_->getInt() );
00175 context_->queueRender();
00176 }
00177
00178 void GridDisplay::updateLineWidth()
00179 {
00180 grid_->setLineWidth( line_width_property_->getFloat() );
00181 context_->queueRender();
00182 }
00183
00184 void GridDisplay::updateHeight()
00185 {
00186 grid_->setHeight( height_property_->getInt() );
00187 context_->queueRender();
00188 }
00189
00190 void GridDisplay::updateStyle()
00191 {
00192 Grid::Style style = (Grid::Style) style_property_->getOptionInt();
00193 grid_->setStyle( style );
00194
00195 switch( style )
00196 {
00197 case Grid::Billboards:
00198 line_width_property_->show();
00199 break;
00200 case Grid::Lines:
00201 default:
00202 line_width_property_->hide();
00203 break;
00204 }
00205 context_->queueRender();
00206 }
00207
00208 void GridDisplay::updateOffset()
00209 {
00210 grid_->getSceneNode()->setPosition( offset_property_->getVector() );
00211 context_->queueRender();
00212 }
00213
00214 void GridDisplay::updatePlane()
00215 {
00216 Ogre::Quaternion orient;
00217 switch( (Plane) plane_property_->getOptionInt() )
00218 {
00219 case XZ:
00220 orient = Ogre::Quaternion( 1, 0, 0, 0 );
00221 break;
00222 case YZ:
00223 orient = Ogre::Quaternion( Ogre::Vector3( 0, -1, 0 ), Ogre::Vector3( 0, 0, 1 ), Ogre::Vector3( 1, 0, 0 ));
00224 break;
00225 case XY:
00226 default:
00227 orient = Ogre::Quaternion( Ogre::Vector3( 1, 0, 0 ), Ogre::Vector3( 0, 0, -1 ), Ogre::Vector3( 0, 1, 0 ));
00228 break;
00229 }
00230 grid_->getSceneNode()->setOrientation( orient );
00231
00232 context_->queueRender();
00233 }
00234
00235 }
00236
00237 #include <pluginlib/class_list_macros.h>
00238 PLUGINLIB_EXPORT_CLASS( rviz::GridDisplay, rviz::Display )