path_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 <boost/bind.hpp>
00031 
00032 #include <OgreSceneNode.h>
00033 #include <OgreSceneManager.h>
00034 #include <OgreManualObject.h>
00035 #include <OgreBillboardSet.h>
00036 #include <OgreMatrix4.h>
00037 
00038 #include <tf/transform_listener.h>
00039 
00040 #include "rviz/display_context.h"
00041 #include "rviz/frame_manager.h"
00042 #include "rviz/properties/enum_property.h"
00043 #include "rviz/properties/color_property.h"
00044 #include "rviz/properties/float_property.h"
00045 #include "rviz/properties/int_property.h"
00046 #include "rviz/properties/vector_property.h"
00047 #include "rviz/validate_floats.h"
00048 
00049 #include "rviz/ogre_helpers/billboard_line.h"
00050 #include "rviz/default_plugin/path_display.h"
00051 
00052 namespace rviz
00053 {
00054 
00055 PathDisplay::PathDisplay()
00056 {
00057   style_property_ = new EnumProperty( "Line Style", "Lines",
00058                                       "The rendering operation to use to draw the grid lines.",
00059                                       this, SLOT( updateStyle() ));
00060 
00061   style_property_->addOption( "Lines", LINES );
00062   style_property_->addOption( "Billboards", BILLBOARDS );
00063 
00064   line_width_property_ = new FloatProperty( "Line Width", 0.03,
00065                                             "The width, in meters, of each path line."
00066                                             "Only works with the 'Billboards' style.",
00067                                             this, SLOT( updateLineWidth() ), this );
00068   line_width_property_->setMin( 0.001 );
00069   line_width_property_->hide();
00070 
00071   color_property_ = new ColorProperty( "Color", QColor( 25, 255, 0 ),
00072                                        "Color to draw the path.", this );
00073 
00074   alpha_property_ = new FloatProperty( "Alpha", 1.0,
00075                                        "Amount of transparency to apply to the path.", this );
00076 
00077   buffer_length_property_ = new IntProperty( "Buffer Length", 1,
00078                                              "Number of paths to display.",
00079                                              this, SLOT( updateBufferLength() ));
00080   buffer_length_property_->setMin( 1 );
00081 
00082   offset_property_ = new VectorProperty( "Offset", Ogre::Vector3::ZERO,
00083                                          "Allows you to offset the path from the origin of the reference frame.  In meters.",
00084                                          this, SLOT( updateOffset() ));
00085 
00086   pose_style_property_ = new EnumProperty( "Pose Style", "None", "Shape to display the pose as.",
00087                                            this, SLOT( updatePoseStyle() ));
00088   pose_style_property_->addOption( "None", NONE );
00089   pose_style_property_->addOption( "Axes", AXES );
00090   pose_style_property_->addOption( "Arrows", ARROWS );
00091 
00092   pose_axes_length_property_ = new rviz::FloatProperty( "Length", 0.3,
00093                                                         "Length of the axes.",
00094                                                         this, SLOT(updatePoseAxisGeometry()) );
00095   pose_axes_radius_property_ = new rviz::FloatProperty( "Radius", 0.03,
00096                                                         "Radius of the axes.",
00097                                                         this, SLOT(updatePoseAxisGeometry()) );
00098 
00099   pose_arrow_color_property_ = new ColorProperty( "Pose Color",
00100                                                   QColor( 255, 85, 255 ),
00101                                                   "Color to draw the poses.",
00102                                                   this, SLOT(updatePoseArrowColor()));
00103   pose_arrow_shaft_length_property_ = new rviz::FloatProperty( "Shaft Length", 0.1,
00104                                                                "Length of the arrow shaft.",
00105                                                                this,
00106                                                                SLOT(updatePoseArrowGeometry()));
00107   pose_arrow_head_length_property_ = new rviz::FloatProperty( "Head Length", 0.2,
00108                                                               "Length of the arrow head.",
00109                                                               this,
00110                                                               SLOT(updatePoseArrowGeometry()));
00111   pose_arrow_shaft_diameter_property_ = new rviz::FloatProperty( "Shaft Diameter", 0.1,
00112                                                                  "Diameter of the arrow shaft.",
00113                                                                  this,
00114                                                                  SLOT(updatePoseArrowGeometry()));
00115   pose_arrow_head_diameter_property_ = new rviz::FloatProperty( "Head Diameter", 0.3,
00116                                                                 "Diameter of the arrow head.",
00117                                                                 this,
00118                                                                 SLOT(updatePoseArrowGeometry()));
00119   pose_axes_length_property_->hide();
00120   pose_axes_radius_property_->hide();
00121   pose_arrow_color_property_->hide();
00122   pose_arrow_shaft_length_property_->hide();
00123   pose_arrow_head_length_property_->hide();
00124   pose_arrow_shaft_diameter_property_->hide();
00125   pose_arrow_head_diameter_property_->hide();
00126 }
00127 
00128 PathDisplay::~PathDisplay()
00129 {
00130   destroyObjects();
00131   destroyPoseAxesChain();
00132   destroyPoseArrowChain();
00133 }
00134 
00135 void PathDisplay::onInitialize()
00136 {
00137   MFDClass::onInitialize();
00138   updateBufferLength();
00139 }
00140 
00141 void PathDisplay::reset()
00142 {
00143   MFDClass::reset();
00144   updateBufferLength();
00145 }
00146 
00147 
00148 void PathDisplay::allocateAxesVector(std::vector<rviz::Axes*>& axes_vect, int num)
00149 {
00150   if (num > axes_vect.size()) {
00151     for (size_t i = axes_vect.size(); i < num; i++) {
00152       rviz::Axes* axes = new rviz::Axes( scene_manager_, scene_node_,
00153                                          pose_axes_length_property_->getFloat(),
00154                                          pose_axes_radius_property_->getFloat());
00155       axes_vect.push_back(axes);
00156     }
00157   }
00158   else if (num < axes_vect.size()) {
00159     for (int i = axes_vect.size() - 1; num <= i; i--) {
00160       delete axes_vect[i];
00161     }
00162     axes_vect.resize(num);
00163   }
00164 }
00165 
00166 void PathDisplay::allocateArrowVector(std::vector<rviz::Arrow*>& arrow_vect, int num)
00167 {
00168   if (num > arrow_vect.size()) {
00169     for (size_t i = arrow_vect.size(); i < num; i++) {
00170       rviz::Arrow* arrow = new rviz::Arrow( scene_manager_, scene_node_ );
00171       arrow_vect.push_back(arrow);
00172     }
00173   }
00174   else if (num < arrow_vect.size()) {
00175     for (int i = arrow_vect.size() - 1; num <= i; i--) {
00176       delete arrow_vect[i];
00177     }
00178     arrow_vect.resize(num);
00179   }
00180 }
00181 
00182 void PathDisplay::destroyPoseAxesChain()
00183 {
00184   for( size_t i = 0; i < axes_chain_.size(); i++ )
00185   {
00186     allocateAxesVector(axes_chain_[i], 0);
00187   }
00188   axes_chain_.resize(0);
00189 }
00190 
00191 void PathDisplay::destroyPoseArrowChain()
00192 {
00193   for( size_t i = 0; i < arrow_chain_.size(); i++ )
00194   {
00195     allocateArrowVector(arrow_chain_[i], 0);
00196   }
00197   arrow_chain_.resize(0);
00198 }
00199 
00200 void PathDisplay::updateStyle()
00201 {
00202   LineStyle style = (LineStyle) style_property_->getOptionInt();
00203 
00204   switch( style )
00205   {
00206   case LINES:
00207   default:
00208     line_width_property_->hide();
00209     break;
00210 
00211   case BILLBOARDS:
00212     line_width_property_->show();
00213     break;
00214   }
00215 
00216   updateBufferLength();
00217 }
00218 
00219 void PathDisplay::updateLineWidth()
00220 {
00221   LineStyle style = (LineStyle) style_property_->getOptionInt();
00222   float line_width = line_width_property_->getFloat();
00223 
00224   if(style == BILLBOARDS) {
00225     for( size_t i = 0; i < billboard_lines_.size(); i++ )
00226     {
00227       rviz::BillboardLine* billboard_line = billboard_lines_[ i ];
00228       if( billboard_line ) billboard_line->setLineWidth( line_width );
00229     }
00230   }
00231   context_->queueRender();
00232 }
00233 
00234 void PathDisplay::updateOffset()
00235 {
00236   scene_node_->setPosition( offset_property_->getVector() );
00237   context_->queueRender();
00238 }
00239 
00240 void PathDisplay::updatePoseStyle()
00241 {
00242   PoseStyle pose_style = (PoseStyle) pose_style_property_->getOptionInt();
00243   switch (pose_style)
00244   {
00245     case AXES:
00246       pose_axes_length_property_->show();
00247       pose_axes_radius_property_->show();
00248       pose_arrow_color_property_->hide();
00249       pose_arrow_shaft_length_property_->hide();
00250       pose_arrow_head_length_property_->hide();
00251       pose_arrow_shaft_diameter_property_->hide();
00252       pose_arrow_head_diameter_property_->hide();
00253       break;
00254     case ARROWS:
00255       pose_axes_length_property_->hide();
00256       pose_axes_radius_property_->hide();
00257       pose_arrow_color_property_->show();
00258       pose_arrow_shaft_length_property_->show();
00259       pose_arrow_head_length_property_->show();
00260       pose_arrow_shaft_diameter_property_->show();
00261       pose_arrow_head_diameter_property_->show();
00262       break;
00263     default:
00264       pose_axes_length_property_->hide();
00265       pose_axes_radius_property_->hide();
00266       pose_arrow_color_property_->hide();
00267       pose_arrow_shaft_length_property_->hide();
00268       pose_arrow_head_length_property_->hide();
00269       pose_arrow_shaft_diameter_property_->hide();
00270       pose_arrow_head_diameter_property_->hide();
00271   }
00272   updateBufferLength();
00273 }
00274 
00275 void PathDisplay::updatePoseAxisGeometry()
00276 {
00277   for(size_t i = 0; i < axes_chain_.size() ; i++)
00278   {
00279     std::vector<rviz::Axes*>& axes_vect = axes_chain_[i];
00280     for(size_t j = 0; j < axes_vect.size() ; j++)
00281     {
00282       axes_vect[j]->set( pose_axes_length_property_->getFloat(),
00283                          pose_axes_radius_property_->getFloat() );
00284     }
00285   }
00286   context_->queueRender();
00287 }
00288 
00289 void PathDisplay::updatePoseArrowColor()
00290 {
00291   QColor color = pose_arrow_color_property_->getColor();
00292 
00293   for( size_t i = 0; i < arrow_chain_.size(); i++ )
00294   {
00295     std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
00296     for( size_t j = 0; j < arrow_vect.size(); j++ )
00297     {
00298       arrow_vect[j]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
00299     }
00300   }  
00301   context_->queueRender();
00302 }
00303 
00304 void PathDisplay::updatePoseArrowGeometry()
00305 {
00306   for( size_t i = 0; i < arrow_chain_.size(); i++ )
00307   {
00308     std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
00309     for( size_t j = 0; j < arrow_vect.size(); j++ )
00310     {
00311       arrow_vect[j]->set(pose_arrow_shaft_length_property_->getFloat(),
00312                          pose_arrow_shaft_diameter_property_->getFloat(), 
00313                          pose_arrow_head_length_property_->getFloat(),
00314                          pose_arrow_head_diameter_property_->getFloat());
00315     }
00316   }  
00317   context_->queueRender();
00318 }
00319 
00320 void PathDisplay::destroyObjects()
00321 {
00322   // Destroy all simple lines, if any
00323   for( size_t i = 0; i < manual_objects_.size(); i++ )
00324   {
00325     Ogre::ManualObject*& manual_object = manual_objects_[ i ];
00326     if( manual_object )
00327     {
00328       manual_object->clear();
00329       scene_manager_->destroyManualObject( manual_object );
00330       manual_object = NULL; // ensure it doesn't get destroyed again
00331     }
00332   }
00333 
00334   // Destroy all billboards, if any
00335   for( size_t i = 0; i < billboard_lines_.size(); i++ )
00336   {
00337     rviz::BillboardLine*& billboard_line = billboard_lines_[ i ];
00338     if( billboard_line )
00339     {
00340       delete billboard_line; // also destroys the corresponding scene node
00341       billboard_line = NULL; // ensure it doesn't get destroyed again
00342     }
00343   }
00344 }
00345 
00346 void PathDisplay::updateBufferLength()
00347 {
00348   // Delete old path objects
00349   destroyObjects();
00350 
00351   // Destroy all axes and arrows
00352   destroyPoseAxesChain();
00353   destroyPoseArrowChain();
00354 
00355   // Read options
00356   int buffer_length = buffer_length_property_->getInt();
00357   LineStyle style = (LineStyle) style_property_->getOptionInt();
00358 
00359   // Create new path objects
00360   switch(style)
00361   {
00362   case LINES: // simple lines with fixed width of 1px
00363     manual_objects_.resize( buffer_length );
00364     for( size_t i = 0; i < manual_objects_.size(); i++ )
00365     {
00366       Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
00367       manual_object->setDynamic( true );
00368       scene_node_->attachObject( manual_object );
00369 
00370       manual_objects_[ i ] = manual_object;
00371     }
00372     break;
00373 
00374   case BILLBOARDS: // billboards with configurable width
00375     billboard_lines_.resize( buffer_length );
00376     for( size_t i = 0; i < billboard_lines_.size(); i++ )
00377     {
00378       rviz::BillboardLine* billboard_line = new rviz::BillboardLine(scene_manager_, scene_node_);
00379       billboard_lines_[ i ] = billboard_line;
00380     }
00381     break;
00382   }
00383   axes_chain_.resize( buffer_length );
00384   arrow_chain_.resize( buffer_length );
00385 
00386 
00387 }
00388 
00389 bool validateFloats( const nav_msgs::Path& msg )
00390 {
00391   bool valid = true;
00392   valid = valid && validateFloats( msg.poses );
00393   return valid;
00394 }
00395 
00396 void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
00397 {
00398   // Calculate index of oldest element in cyclic buffer
00399   size_t bufferIndex = messages_received_ % buffer_length_property_->getInt();
00400 
00401   LineStyle style = (LineStyle) style_property_->getOptionInt();
00402   Ogre::ManualObject* manual_object = NULL;
00403   rviz::BillboardLine* billboard_line = NULL;
00404 
00405   // Delete oldest element
00406   switch(style)
00407   {
00408   case LINES:
00409     manual_object = manual_objects_[ bufferIndex ];
00410     manual_object->clear();
00411     break;
00412 
00413   case BILLBOARDS:
00414     billboard_line = billboard_lines_[ bufferIndex ];
00415     billboard_line->clear();
00416     break;
00417   }
00418 
00419   // Check if path contains invalid coordinate values
00420   if( !validateFloats( *msg ))
00421   {
00422     setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00423     return;
00424   }
00425 
00426   // Lookup transform into fixed frame
00427   Ogre::Vector3 position;
00428   Ogre::Quaternion orientation;
00429   if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00430   {
00431     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00432   }
00433 
00434   Ogre::Matrix4 transform( orientation );
00435   transform.setTrans( position );
00436 
00437 //  scene_node_->setPosition( position );
00438 //  scene_node_->setOrientation( orientation );
00439 
00440   Ogre::ColourValue color = color_property_->getOgreColor();
00441   color.a = alpha_property_->getFloat();
00442 
00443   uint32_t num_points = msg->poses.size();
00444   float line_width = line_width_property_->getFloat();
00445 
00446   switch(style)
00447   {
00448   case LINES:
00449     manual_object->estimateVertexCount( num_points );
00450     manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
00451     for( uint32_t i=0; i < num_points; ++i)
00452     {
00453       const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
00454       Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
00455       manual_object->position( xpos.x, xpos.y, xpos.z );
00456       manual_object->colour( color );
00457     }
00458 
00459     manual_object->end();
00460     break;
00461 
00462   case BILLBOARDS:
00463     billboard_line->setNumLines( 1 );
00464     billboard_line->setMaxPointsPerLine( num_points );
00465     billboard_line->setLineWidth( line_width );
00466 
00467     for( uint32_t i=0; i < num_points; ++i)
00468     {
00469       const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
00470       Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
00471       billboard_line->addPoint( xpos, color );
00472     }
00473 
00474     break;
00475   }
00476 
00477   // process pose markers
00478   PoseStyle pose_style = (PoseStyle) pose_style_property_->getOptionInt();
00479   std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[ bufferIndex ];
00480   std::vector<rviz::Axes*>& axes_vect = axes_chain_[ bufferIndex ];
00481 
00482   switch(pose_style)
00483   {
00484     case AXES:
00485       allocateAxesVector(axes_vect, num_points);
00486       for( uint32_t i=0; i < num_points; ++i)
00487       {
00488         const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
00489         Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
00490         axes_vect[i]->setPosition(xpos);
00491         Ogre::Quaternion orientation(msg->poses[ i ].pose.orientation.w,
00492                                      msg->poses[ i ].pose.orientation.x,
00493                                      msg->poses[ i ].pose.orientation.y,
00494                                      msg->poses[ i ].pose.orientation.z);
00495         axes_vect[i]->setOrientation(orientation);
00496       }
00497       break;
00498 
00499     case ARROWS:
00500       allocateArrowVector(arrow_vect, num_points);
00501       for( uint32_t i=0; i < num_points; ++i)
00502       {
00503         const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
00504         Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
00505 
00506         QColor color = pose_arrow_color_property_->getColor();
00507         arrow_vect[i]->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
00508 
00509         arrow_vect[i]->set(pose_arrow_shaft_length_property_->getFloat(),
00510                            pose_arrow_shaft_diameter_property_->getFloat(), 
00511                            pose_arrow_head_length_property_->getFloat(),
00512                            pose_arrow_head_diameter_property_->getFloat());
00513         arrow_vect[i]->setPosition(xpos);
00514         Ogre::Quaternion orientation(msg->poses[ i ].pose.orientation.w,
00515                                      msg->poses[ i ].pose.orientation.x,
00516                                      msg->poses[ i ].pose.orientation.y,
00517                                      msg->poses[ i ].pose.orientation.z);
00518       
00519         Ogre::Vector3 dir(1, 0, 0);
00520         dir = orientation * dir;
00521         arrow_vect[i]->setDirection(dir);
00522       }
00523       break;
00524 
00525     default:
00526       break;
00527   }
00528   context_->queueRender();
00529 
00530 }
00531 
00532 } // namespace rviz
00533 
00534 #include <pluginlib/class_list_macros.h>
00535 PLUGINLIB_EXPORT_CLASS( rviz::PathDisplay, rviz::Display )


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