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 <OGRE/OgreSceneNode.h>
00035 #include <OGRE/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
00043 #include "grid_display.h"
00044
00045 namespace rviz
00046 {
00047
00048 GridDisplay::GridDisplay()
00049 : Display()
00050 , scene_node_( 0 )
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 delete grid_;
00108 scene_manager_->destroySceneNode(scene_node_);
00109 }
00110
00111 void GridDisplay::onInitialize()
00112 {
00113 QColor color = color_property_->getColor();
00114 color.setAlphaF( alpha_property_->getFloat() );
00115
00116 frame_property_->setFrameManager( context_->getFrameManager() );
00117 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00118 grid_ = new Grid( scene_manager_, scene_node_,
00119 (Grid::Style) style_property_->getOptionInt(),
00120 cell_count_property_->getInt(),
00121 cell_size_property_->getFloat(),
00122 line_width_property_->getFloat(),
00123 qtToOgre( color ));
00124
00125 grid_->getSceneNode()->setVisible( false );
00126 updatePlane();
00127 }
00128
00129 void GridDisplay::onEnable()
00130 {
00131 grid_->getSceneNode()->setVisible( true );
00132 }
00133
00134 void GridDisplay::onDisable()
00135 {
00136 grid_->getSceneNode()->setVisible( false );
00137 }
00138
00139 void GridDisplay::update(float dt, float ros_dt)
00140 {
00141 QString qframe = frame_property_->getFrame();
00142 std::string frame = qframe.toStdString();
00143
00144 Ogre::Vector3 position;
00145 Ogre::Quaternion orientation;
00146 if( context_->getFrameManager()->getTransform( frame, ros::Time(), position, orientation ))
00147 {
00148 scene_node_->setPosition( position );
00149 scene_node_->setOrientation( orientation );
00150 setStatus( StatusProperty::Ok, "Transform", "Transform OK" );
00151 }
00152 else
00153 {
00154 std::string error;
00155 if( context_->getFrameManager()->transformHasProblems( frame, ros::Time(), error ))
00156 {
00157 setStatus( StatusProperty::Error, "Transform", QString::fromStdString( error ));
00158 }
00159 else
00160 {
00161 setStatus( StatusProperty::Error, "Transform",
00162 "Could not transform from [" + qframe + "] to [" + fixed_frame_ + "]" );
00163 }
00164 }
00165 }
00166
00167 void GridDisplay::updateColor()
00168 {
00169 QColor color = color_property_->getColor();
00170 color.setAlphaF( alpha_property_->getFloat() );
00171 grid_->setColor( qtToOgre( color ));
00172 context_->queueRender();
00173 }
00174
00175 void GridDisplay::updateCellSize()
00176 {
00177 grid_->setCellLength( cell_size_property_->getFloat() );
00178 context_->queueRender();
00179 }
00180
00181 void GridDisplay::updateCellCount()
00182 {
00183 grid_->setCellCount( cell_count_property_->getInt() );
00184 context_->queueRender();
00185 }
00186
00187 void GridDisplay::updateLineWidth()
00188 {
00189 grid_->setLineWidth( line_width_property_->getFloat() );
00190 context_->queueRender();
00191 }
00192
00193 void GridDisplay::updateHeight()
00194 {
00195 grid_->setHeight( height_property_->getInt() );
00196 context_->queueRender();
00197 }
00198
00199 void GridDisplay::updateStyle()
00200 {
00201 Grid::Style style = (Grid::Style) style_property_->getOptionInt();
00202 grid_->setStyle( style );
00203
00204 switch( style )
00205 {
00206 case Grid::Billboards:
00207 line_width_property_->show();
00208 break;
00209 case Grid::Lines:
00210 default:
00211 line_width_property_->hide();
00212 break;
00213 }
00214 context_->queueRender();
00215 }
00216
00217 void GridDisplay::updateOffset()
00218 {
00219 grid_->getSceneNode()->setPosition( offset_property_->getVector() );
00220 context_->queueRender();
00221 }
00222
00223 void GridDisplay::updatePlane()
00224 {
00225 Ogre::Quaternion orient;
00226 switch( (Plane) plane_property_->getOptionInt() )
00227 {
00228 case XZ:
00229 orient = Ogre::Quaternion( 1, 0, 0, 0 );
00230 break;
00231 case YZ:
00232 orient = Ogre::Quaternion( Ogre::Vector3( 0, -1, 0 ), Ogre::Vector3( 0, 0, 1 ), Ogre::Vector3( 1, 0, 0 ));
00233 break;
00234 case XY:
00235 default:
00236 orient = Ogre::Quaternion( Ogre::Vector3( 1, 0, 0 ), Ogre::Vector3( 0, 0, -1 ), Ogre::Vector3( 0, 1, 0 ));
00237 break;
00238 }
00239 grid_->getSceneNode()->setOrientation( orient );
00240
00241 context_->queueRender();
00242 }
00243
00244 }
00245
00246 #include <pluginlib/class_list_macros.h>
00247 PLUGINLIB_DECLARE_CLASS( rviz, Grid, rviz::GridDisplay, rviz::Display )