grid_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 } // namespace rviz
00236 
00237 #include <pluginlib/class_list_macros.h>
00238 PLUGINLIB_EXPORT_CLASS( rviz::GridDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:15