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->setWrench( msg->wrench );
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 )


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