point_display.cpp
Go to the documentation of this file.
00001 #include <OgreSceneNode.h>
00002 #include <OgreSceneManager.h>
00003 
00004 #include <rviz/visualization_manager.h>
00005 #include <rviz/properties/color_property.h>
00006 #include <rviz/properties/float_property.h>
00007 #include <rviz/properties/int_property.h>
00008 #include <rviz/frame_manager.h>
00009 #include <rviz/validate_floats.h>
00010 
00011 #include "point_visual.h"
00012 
00013 #include "point_display.h"
00014 
00015 namespace rviz
00016 {
00017 
00018     PointStampedDisplay::PointStampedDisplay()
00019     {
00020         color_property_ =
00021             new rviz::ColorProperty( "Color", QColor(204, 41, 204),
00022                                      "Color of a point",
00023                                      this, SLOT( updateColorAndAlpha() ));
00024 
00025         alpha_property_ =
00026             new rviz::FloatProperty( "Alpha", 1.0,
00027                                      "0 is fully transparent, 1.0 is fully opaque.",
00028                                      this, SLOT( updateColorAndAlpha() ));
00029 
00030         radius_property_ =
00031             new rviz::FloatProperty( "Radius", 0.2,
00032                                      "Radius of a point",
00033                                      this, SLOT( updateColorAndAlpha() ));
00034 
00035         history_length_property_ =
00036             new rviz::IntProperty( "History Length", 1,
00037                                    "Number of prior measurements to display.",
00038                                    this, SLOT( updateHistoryLength() ));
00039         history_length_property_->setMin( 1 );
00040         history_length_property_->setMax( 100000 );
00041     }
00042 
00043     void PointStampedDisplay::onInitialize()
00044     {
00045         MFDClass::onInitialize();
00046         updateHistoryLength();
00047     }
00048 
00049     PointStampedDisplay::~PointStampedDisplay()
00050     {
00051     }
00052 
00053     // Clear the visuals by deleting their objects.
00054     void PointStampedDisplay::reset()
00055     {
00056         MFDClass::reset();
00057         visuals_.clear();
00058     }
00059 
00060     // Set the current color and alpha values for each visual.
00061     void PointStampedDisplay::updateColorAndAlpha()
00062     {
00063         float alpha = alpha_property_->getFloat();
00064         float radius = radius_property_->getFloat();
00065         Ogre::ColourValue color = color_property_->getOgreColor();
00066 
00067         for( size_t i = 0; i < visuals_.size(); i++ )
00068         {
00069             visuals_[i]->setColor( color.r, color.g, color.b, alpha );
00070             visuals_[i]->setRadius( radius );
00071         }
00072     }
00073 
00074     // Set the number of past visuals to show.
00075     void PointStampedDisplay::updateHistoryLength()
00076     {
00077         visuals_.rset_capacity(history_length_property_->getInt());
00078     }
00079 
00080     // This is our callback to handle an incoming message.
00081     void PointStampedDisplay::processMessage( const geometry_msgs::PointStamped::ConstPtr& msg )
00082     {
00083 
00084         if( !rviz::validateFloats( msg->point ))
00085             {
00086                 setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00087                 return;
00088             }
00089 
00090         // Here we call the rviz::FrameManager to get the transform from the
00091         // fixed frame to the frame in the header of this Point message.  If
00092         // it fails, we can't do anything else so we return.
00093         Ogre::Quaternion orientation;
00094         Ogre::Vector3 position;
00095         if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
00096                                                         msg->header.stamp,
00097                                                         position, orientation ))
00098         {
00099             ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00100                        msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ) );
00101             return;
00102         }
00103 
00104         // We are keeping a circular buffer of visual pointers.  This gets
00105         // the next one, or creates and stores it if the buffer is not full
00106         boost::shared_ptr<PointStampedVisual> visual;
00107         if( visuals_.full() )
00108             {
00109                 visual = visuals_.front();
00110             }
00111         else
00112             {
00113                 visual.reset(new PointStampedVisual( context_->getSceneManager(), scene_node_ ));
00114             }
00115 
00116 
00117         // Now set or update the contents of the chosen visual.
00118         visual->setMessage( msg );
00119         visual->setFramePosition( position );
00120         visual->setFrameOrientation( orientation );
00121         float alpha = alpha_property_->getFloat();
00122         float radius = radius_property_->getFloat();
00123         Ogre::ColourValue color = color_property_->getOgreColor();
00124         visual->setColor( color.r,  color.g,  color.b, alpha);
00125         visual->setRadius( radius );
00126 
00127 
00128         // And send it to the end of the circular buffer
00129         visuals_.push_back(visual);
00130     }
00131 
00132 } // end namespace rviz
00133 
00134 // Tell pluginlib about this class.  It is important to do this in
00135 // global scope, outside our package's namespace.
00136 #include <pluginlib/class_list_macros.h>
00137 PLUGINLIB_EXPORT_CLASS( rviz::PointStampedDisplay, rviz::Display )
00138 
00139 


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27