wrench_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/frame_manager.h>
00006 #include <rviz/properties/color_property.h>
00007 #include <rviz/properties/float_property.h>
00008 #include <rviz/properties/int_property.h>
00009 #include <rviz/properties/parse_color.h>
00010 #include <rviz/validate_floats.h>
00011 
00012 #include <boost/foreach.hpp>
00013 
00014 #include "wrench_visual.h"
00015 
00016 #include "wrench_display.h"
00017 
00018 namespace rviz
00019 {
00020 
00021     WrenchStampedDisplay::WrenchStampedDisplay()
00022     {
00023         force_color_property_ =
00024             new rviz::ColorProperty( "Force Color", QColor( 204, 51, 51 ),
00025                                      "Color to draw the force arrows.",
00026                                      this, SLOT( updateColorAndAlpha() ));
00027 
00028         torque_color_property_ =
00029             new rviz::ColorProperty( "Torque Color", QColor( 204, 204, 51),
00030                                      "Color to draw the torque arrows.",
00031                                      this, SLOT( updateColorAndAlpha() ));
00032 
00033         alpha_property_ =
00034             new rviz::FloatProperty( "Alpha", 1.0,
00035                                      "0 is fully transparent, 1.0 is fully opaque.",
00036                                      this, SLOT( updateColorAndAlpha() ));
00037 
00038         force_scale_property_ =
00039             new rviz::FloatProperty( "Force Arrow Scale", 2.0,
00040                                      "force arrow scale",
00041                                      this, SLOT( updateColorAndAlpha() ));
00042 
00043         torque_scale_property_ =
00044             new rviz::FloatProperty( "Torque Arrow Scale", 2.0,
00045                                      "torque arrow scale",
00046                                      this, SLOT( updateColorAndAlpha() ));            
00047 
00048         width_property_ =
00049             new rviz::FloatProperty( "Arrow Width", 0.5,
00050                                      "arrow width",
00051                                      this, SLOT( updateColorAndAlpha() ));
00052 
00053 
00054         history_length_property_ =
00055             new rviz::IntProperty( "History Length", 1,
00056                                    "Number of prior measurements to display.",
00057                                    this, SLOT( updateHistoryLength() ));
00058 
00059         history_length_property_->setMin( 1 );
00060         history_length_property_->setMax( 100000 );
00061     }
00062 
00063     void WrenchStampedDisplay::onInitialize()
00064     {
00065         MFDClass::onInitialize();
00066         updateHistoryLength( );
00067     }
00068 
00069     WrenchStampedDisplay::~WrenchStampedDisplay()
00070     {
00071     }
00072 
00073     // Override rviz::Display's reset() function to add a call to clear().
00074     void WrenchStampedDisplay::reset()
00075     {
00076         MFDClass::reset();
00077         visuals_.clear();
00078     }
00079 
00080     void WrenchStampedDisplay::updateColorAndAlpha()
00081     {
00082         float alpha = alpha_property_->getFloat();
00083         float force_scale = force_scale_property_->getFloat();
00084         float torque_scale = torque_scale_property_->getFloat();
00085         float width = width_property_->getFloat();
00086         Ogre::ColourValue force_color = force_color_property_->getOgreColor();
00087         Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();
00088 
00089         for( size_t i = 0; i < visuals_.size(); i++ )
00090         {
00091             visuals_[i]->setForceColor( force_color.r, force_color.g, force_color.b, alpha );
00092             visuals_[i]->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha );
00093             visuals_[i]->setForceScale( force_scale );
00094             visuals_[i]->setTorqueScale( torque_scale );
00095             visuals_[i]->setWidth( width );
00096         }
00097     }
00098 
00099     // Set the number of past visuals to show.
00100     void WrenchStampedDisplay::updateHistoryLength()
00101     {
00102         visuals_.rset_capacity(history_length_property_->getInt());
00103     }
00104 
00105     bool validateFloats( const geometry_msgs::WrenchStamped& msg )
00106     {
00107         return rviz::validateFloats(msg.wrench.force) && rviz::validateFloats(msg.wrench.torque) ;
00108     }
00109 
00110     // This is our callback to handle an incoming message.
00111     void WrenchStampedDisplay::processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg )
00112     {
00113         if( !validateFloats( *msg ))
00114             {
00115                 setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00116                 return;
00117             }
00118 
00119         // Here we call the rviz::FrameManager to get the transform from the
00120         // fixed frame to the frame in the header of this Imu message.  If
00121         // it fails, we can't do anything else so we return.
00122         Ogre::Quaternion orientation;
00123         Ogre::Vector3 position;
00124         if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
00125                                                         msg->header.stamp,
00126                                                         position, orientation ))
00127           {
00128             ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00129                        msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00130             return;
00131           }
00132 
00133         if ( position.isNaN() )
00134           {
00135             ROS_ERROR_THROTTLE(1.0, "Wrench position contains NaNs. Skipping render as long as the position is invalid");
00136             return;
00137           }
00138 
00139         // We are keeping a circular buffer of visual pointers.  This gets
00140         // the next one, or creates and stores it if the buffer is not full
00141         boost::shared_ptr<WrenchStampedVisual> visual;
00142         if( visuals_.full() )
00143             {
00144                 visual = visuals_.front();
00145             }
00146         else
00147             {
00148                 visual.reset(new WrenchStampedVisual( context_->getSceneManager(), scene_node_ ));
00149             }
00150 
00151         // Now set or update the contents of the chosen visual.
00152         visual->setMessage( msg );
00153         visual->setFramePosition( position );
00154         visual->setFrameOrientation( orientation );
00155         float alpha = alpha_property_->getFloat();
00156         float force_scale = force_scale_property_->getFloat();
00157         float torque_scale = torque_scale_property_->getFloat();
00158         float width = width_property_->getFloat();
00159         Ogre::ColourValue force_color = force_color_property_->getOgreColor();
00160         Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();
00161         visual->setForceColor( force_color.r, force_color.g, force_color.b, alpha );
00162         visual->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha );
00163         visual->setForceScale( force_scale );
00164         visual->setTorqueScale( torque_scale );
00165         visual->setWidth( width );
00166 
00167         // And send it to the end of the circular buffer
00168         visuals_.push_back(visual);
00169     }
00170 
00171 } // end namespace rviz
00172 
00173 // Tell pluginlib about this class.  It is important to do this in
00174 // global scope, outside our package's namespace.
00175 #include <pluginlib/class_list_macros.h>
00176 PLUGINLIB_EXPORT_CLASS( rviz::WrenchStampedDisplay, rviz::Display )
00177 
00178 
00179 


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