wrench_display.cpp
Go to the documentation of this file.
1 #include <OgreSceneNode.h>
2 #include <OgreSceneManager.h>
3 
5 #include <rviz/frame_manager.h>
10 #include <rviz/validate_floats.h>
11 
12 #include <boost/foreach.hpp>
13 
14 #include "wrench_visual.h"
15 
16 #include "wrench_display.h"
17 
18 namespace rviz
19 {
20 
22 {
24  new rviz::ColorProperty( "Force Color", QColor( 204, 51, 51 ),
25  "Color to draw the force arrows.",
26  this, SLOT( updateColorAndAlpha() ));
27 
29  new rviz::ColorProperty( "Torque Color", QColor( 204, 204, 51),
30  "Color to draw the torque arrows.",
31  this, SLOT( updateColorAndAlpha() ));
32 
34  new rviz::FloatProperty( "Alpha", 1.0,
35  "0 is fully transparent, 1.0 is fully opaque.",
36  this, SLOT( updateColorAndAlpha() ));
37 
39  new rviz::FloatProperty( "Force Arrow Scale", 2.0,
40  "force arrow scale",
41  this, SLOT( updateColorAndAlpha() ));
42 
44  new rviz::FloatProperty( "Torque Arrow Scale", 2.0,
45  "torque arrow scale",
46  this, SLOT( updateColorAndAlpha() ));
47 
49  new rviz::FloatProperty( "Arrow Width", 0.5,
50  "arrow width",
51  this, SLOT( updateColorAndAlpha() ));
52 
53 
55  new rviz::IntProperty( "History Length", 1,
56  "Number of prior measurements to display.",
57  this, SLOT( updateHistoryLength() ));
58 
61 }
62 
64 {
67 }
68 
70 {
71 }
72 
73 // Override rviz::Display's reset() function to add a call to clear().
75 {
77  visuals_.clear();
78 }
79 
81 {
82  float alpha = alpha_property_->getFloat();
83  float force_scale = force_scale_property_->getFloat();
84  float torque_scale = torque_scale_property_->getFloat();
85  float width = width_property_->getFloat();
86  Ogre::ColourValue force_color = force_color_property_->getOgreColor();
87  Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();
88 
89  for( size_t i = 0; i < visuals_.size(); i++ )
90  {
91  visuals_[i]->setForceColor( force_color.r, force_color.g, force_color.b, alpha );
92  visuals_[i]->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha );
93  visuals_[i]->setForceScale( force_scale );
94  visuals_[i]->setTorqueScale( torque_scale );
95  visuals_[i]->setWidth( width );
96  }
97 }
98 
99 // Set the number of past visuals to show.
101 {
102  visuals_.rset_capacity(history_length_property_->getInt());
103 }
104 
105 bool validateFloats( const geometry_msgs::WrenchStamped& msg )
106 {
107  return rviz::validateFloats(msg.wrench.force) && rviz::validateFloats(msg.wrench.torque) ;
108 }
109 
110 // This is our callback to handle an incoming message.
111 void WrenchStampedDisplay::processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg )
112 {
113  if( !validateFloats( *msg ))
114  {
115  setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
116  return;
117  }
118 
119  // Here we call the rviz::FrameManager to get the transform from the
120  // fixed frame to the frame in the header of this Imu message. If
121  // it fails, we can't do anything else so we return.
122  Ogre::Quaternion orientation;
123  Ogre::Vector3 position;
124  if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
125  msg->header.stamp,
126  position, orientation ))
127  {
128  ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
129  msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
130  return;
131  }
132 
133  if ( position.isNaN() )
134  {
135  ROS_ERROR_THROTTLE(1.0, "Wrench position contains NaNs. Skipping render as long as the position is invalid");
136  return;
137  }
138 
139  // We are keeping a circular buffer of visual pointers. This gets
140  // the next one, or creates and stores it if the buffer is not full
142  if( visuals_.full() )
143  {
144  visual = visuals_.front();
145  }
146  else
147  {
148  visual.reset(new WrenchVisual( context_->getSceneManager(), scene_node_ ));
149  }
150 
151  // Now set or update the contents of the chosen visual.
152  visual->setWrench( msg->wrench );
153  visual->setFramePosition( position );
154  visual->setFrameOrientation( orientation );
155  float alpha = alpha_property_->getFloat();
156  float force_scale = force_scale_property_->getFloat();
157  float torque_scale = torque_scale_property_->getFloat();
158  float width = width_property_->getFloat();
159  Ogre::ColourValue force_color = force_color_property_->getOgreColor();
160  Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();
161  visual->setForceColor( force_color.r, force_color.g, force_color.b, alpha );
162  visual->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha );
163  visual->setForceScale( force_scale );
164  visual->setTorqueScale( torque_scale );
165  visual->setWidth( width );
166 
167  // And send it to the end of the circular buffer
168  visuals_.push_back(visual);
169 }
170 
171 } // end namespace rviz
172 
173 // Tell pluginlib about this class. It is important to do this in
174 // global scope, outside our package's namespace.
rviz::FloatProperty * force_scale_property_
Ogre::ColourValue getOgreColor() const
virtual void onInitialize()
Override this function to do subclass-specific initialization.
boost::circular_buffer< boost::shared_ptr< WrenchVisual > > visuals_
rviz::ColorProperty * force_color_property_
virtual void reset()
Called to tell the display to clear its state.
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
rviz::FloatProperty * alpha_property_
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:73
rviz::IntProperty * history_length_property_
virtual float getFloat() const
void setMin(int min)
rviz::FloatProperty * torque_scale_property_
Property specialized to enforce floating point max/min.
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:38
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:264
void setMax(int max)
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
#define ROS_ERROR_THROTTLE(rate,...)
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
rviz::FloatProperty * width_property_
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
void processMessage(const geometry_msgs::WrenchStamped::ConstPtr &msg)
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
rviz::ColorProperty * torque_color_property_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:186
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:52