Skip to content

Commit

Permalink
generalize WrenchVisual class (ros-visualization#982) (ros-visualizat…
Browse files Browse the repository at this point in the history
…ion#990)

* whitespace normalization in wrench_display.* + wrench_visual.*

* generalized WrenchStampedVisual::setMessage()

- interface changed to receive a Wrench& only
- renamed setMessage() to setWrench()

* alternative API: WrenchStampedVisual::setWrench(OgreVector3 force, OgreVector3 torque)

* expose SceneNode::setVisible()

* separate scene nodes for force and torque marker

... to allow hiding of arrows if they become shorter than wide

* retain API compatibility

* retain ABI compatibility
wjwwood committed Apr 7, 2016

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
1 parent 005d8cc commit c6bfbb6
Showing 4 changed files with 254 additions and 214 deletions.
215 changes: 106 additions & 109 deletions src/rviz/default_plugin/wrench_display.cpp
Original file line number Diff line number Diff line change
@@ -18,162 +18,159 @@
namespace rviz
{

WrenchStampedDisplay::WrenchStampedDisplay()
{
force_color_property_ =
new rviz::ColorProperty( "Force Color", QColor( 204, 51, 51 ),
WrenchStampedDisplay::WrenchStampedDisplay()
{
force_color_property_ =
new rviz::ColorProperty( "Force Color", QColor( 204, 51, 51 ),
"Color to draw the force arrows.",
this, SLOT( updateColorAndAlpha() ));

torque_color_property_ =
new rviz::ColorProperty( "Torque Color", QColor( 204, 204, 51),
torque_color_property_ =
new rviz::ColorProperty( "Torque Color", QColor( 204, 204, 51),
"Color to draw the torque arrows.",
this, SLOT( updateColorAndAlpha() ));

alpha_property_ =
alpha_property_ =
new rviz::FloatProperty( "Alpha", 1.0,
"0 is fully transparent, 1.0 is fully opaque.",
this, SLOT( updateColorAndAlpha() ));

force_scale_property_ =
force_scale_property_ =
new rviz::FloatProperty( "Force Arrow Scale", 2.0,
"force arrow scale",
this, SLOT( updateColorAndAlpha() ));

torque_scale_property_ =
torque_scale_property_ =
new rviz::FloatProperty( "Torque Arrow Scale", 2.0,
"torque arrow scale",
this, SLOT( updateColorAndAlpha() ));
this, SLOT( updateColorAndAlpha() ));

width_property_ =
width_property_ =
new rviz::FloatProperty( "Arrow Width", 0.5,
"arrow width",
this, SLOT( updateColorAndAlpha() ));


history_length_property_ =
new rviz::IntProperty( "History Length", 1,
history_length_property_ =
new rviz::IntProperty( "History Length", 1,
"Number of prior measurements to display.",
this, SLOT( updateHistoryLength() ));

history_length_property_->setMin( 1 );
history_length_property_->setMax( 100000 );
}
history_length_property_->setMin( 1 );
history_length_property_->setMax( 100000 );
}

void WrenchStampedDisplay::onInitialize()
{
MFDClass::onInitialize();
updateHistoryLength( );
}
void WrenchStampedDisplay::onInitialize()
{
MFDClass::onInitialize();
updateHistoryLength( );
}

WrenchStampedDisplay::~WrenchStampedDisplay()
{
}

// Override rviz::Display's reset() function to add a call to clear().
void WrenchStampedDisplay::reset()
{
MFDClass::reset();
visuals_.clear();
}

WrenchStampedDisplay::~WrenchStampedDisplay()
void WrenchStampedDisplay::updateColorAndAlpha()
{
float alpha = alpha_property_->getFloat();
float force_scale = force_scale_property_->getFloat();
float torque_scale = torque_scale_property_->getFloat();
float width = width_property_->getFloat();
Ogre::ColourValue force_color = force_color_property_->getOgreColor();
Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();

for( size_t i = 0; i < visuals_.size(); i++ )
{
visuals_[i]->setForceColor( force_color.r, force_color.g, force_color.b, alpha );
visuals_[i]->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha );
visuals_[i]->setForceScale( force_scale );
visuals_[i]->setTorqueScale( torque_scale );
visuals_[i]->setWidth( width );
}
}

// Set the number of past visuals to show.
void WrenchStampedDisplay::updateHistoryLength()
{
visuals_.rset_capacity(history_length_property_->getInt());
}

// Override rviz::Display's reset() function to add a call to clear().
void WrenchStampedDisplay::reset()
bool validateFloats( const geometry_msgs::WrenchStamped& msg )
{
return rviz::validateFloats(msg.wrench.force) && rviz::validateFloats(msg.wrench.torque) ;
}

// This is our callback to handle an incoming message.
void WrenchStampedDisplay::processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg )
{
if( !validateFloats( *msg ))
{
MFDClass::reset();
visuals_.clear();
setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
return;
}

void WrenchStampedDisplay::updateColorAndAlpha()
// Here we call the rviz::FrameManager to get the transform from the
// fixed frame to the frame in the header of this Imu message. If
// it fails, we can't do anything else so we return.
Ogre::Quaternion orientation;
Ogre::Vector3 position;
if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
msg->header.stamp,
position, orientation ))
{
float alpha = alpha_property_->getFloat();
float force_scale = force_scale_property_->getFloat();
float torque_scale = torque_scale_property_->getFloat();
float width = width_property_->getFloat();
Ogre::ColourValue force_color = force_color_property_->getOgreColor();
Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();

for( size_t i = 0; i < visuals_.size(); i++ )
{
visuals_[i]->setForceColor( force_color.r, force_color.g, force_color.b, alpha );
visuals_[i]->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha );
visuals_[i]->setForceScale( force_scale );
visuals_[i]->setTorqueScale( torque_scale );
visuals_[i]->setWidth( width );
}
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}

// Set the number of past visuals to show.
void WrenchStampedDisplay::updateHistoryLength()
if ( position.isNaN() )
{
visuals_.rset_capacity(history_length_property_->getInt());
ROS_ERROR_THROTTLE(1.0, "Wrench position contains NaNs. Skipping render as long as the position is invalid");
return;
}

bool validateFloats( const geometry_msgs::WrenchStamped& msg )
// We are keeping a circular buffer of visual pointers. This gets
// the next one, or creates and stores it if the buffer is not full
boost::shared_ptr<WrenchStampedVisual> visual;
if( visuals_.full() )
{
return rviz::validateFloats(msg.wrench.force) && rviz::validateFloats(msg.wrench.torque) ;
visual = visuals_.front();
}

// This is our callback to handle an incoming message.
void WrenchStampedDisplay::processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg )
else
{
if( !validateFloats( *msg ))
{
setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
return;
}

// Here we call the rviz::FrameManager to get the transform from the
// fixed frame to the frame in the header of this Imu message. If
// it fails, we can't do anything else so we return.
Ogre::Quaternion orientation;
Ogre::Vector3 position;
if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
msg->header.stamp,
position, orientation ))
{
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}

if ( position.isNaN() )
{
ROS_ERROR_THROTTLE(1.0, "Wrench position contains NaNs. Skipping render as long as the position is invalid");
return;
}

// We are keeping a circular buffer of visual pointers. This gets
// the next one, or creates and stores it if the buffer is not full
boost::shared_ptr<WrenchStampedVisual> visual;
if( visuals_.full() )
{
visual = visuals_.front();
}
else
{
visual.reset(new WrenchStampedVisual( context_->getSceneManager(), scene_node_ ));
}

// Now set or update the contents of the chosen visual.
visual->setMessage( msg );
visual->setFramePosition( position );
visual->setFrameOrientation( orientation );
float alpha = alpha_property_->getFloat();
float force_scale = force_scale_property_->getFloat();
float torque_scale = torque_scale_property_->getFloat();
float width = width_property_->getFloat();
Ogre::ColourValue force_color = force_color_property_->getOgreColor();
Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();
visual->setForceColor( force_color.r, force_color.g, force_color.b, alpha );
visual->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha );
visual->setForceScale( force_scale );
visual->setTorqueScale( torque_scale );
visual->setWidth( width );

// And send it to the end of the circular buffer
visuals_.push_back(visual);
visual.reset(new WrenchStampedVisual( context_->getSceneManager(), scene_node_ ));
}

// Now set or update the contents of the chosen visual.
visual->setWrench( msg->wrench );
visual->setFramePosition( position );
visual->setFrameOrientation( orientation );
float alpha = alpha_property_->getFloat();
float force_scale = force_scale_property_->getFloat();
float torque_scale = torque_scale_property_->getFloat();
float width = width_property_->getFloat();
Ogre::ColourValue force_color = force_color_property_->getOgreColor();
Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();
visual->setForceColor( force_color.r, force_color.g, force_color.b, alpha );
visual->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha );
visual->setForceScale( force_scale );
visual->setTorqueScale( torque_scale );
visual->setWidth( width );

// And send it to the end of the circular buffer
visuals_.push_back(visual);
}

} // end namespace rviz

// Tell pluginlib about this class. It is important to do this in
// global scope, outside our package's namespace.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rviz::WrenchStampedDisplay, rviz::Display )



66 changes: 33 additions & 33 deletions src/rviz/default_plugin/wrench_display.h
Original file line number Diff line number Diff line change
@@ -10,55 +10,55 @@

namespace Ogre
{
class SceneNode;
class SceneNode;
}

namespace rviz
{
class ColorProperty;
class ROSTopicStringProperty;
class FloatProperty;
class IntProperty;
class ColorProperty;
class ROSTopicStringProperty;
class FloatProperty;
class IntProperty;
}

namespace rviz
{

class WrenchStampedVisual;
class WrenchStampedVisual;

class WrenchStampedDisplay: public rviz::MessageFilterDisplay<geometry_msgs::WrenchStamped>
{
class WrenchStampedDisplay: public rviz::MessageFilterDisplay<geometry_msgs::WrenchStamped>
{
Q_OBJECT
public:
// Constructor. pluginlib::ClassLoader creates instances by calling
// the default constructor, so make sure you have one.
WrenchStampedDisplay();
virtual ~WrenchStampedDisplay();
public:
// Constructor. pluginlib::ClassLoader creates instances by calling
// the default constructor, so make sure you have one.
WrenchStampedDisplay();
virtual ~WrenchStampedDisplay();

protected:
// Overrides of public virtual functions from the Display class.
virtual void onInitialize();
virtual void reset();
protected:
// Overrides of public virtual functions from the Display class.
virtual void onInitialize();
virtual void reset();

private Q_SLOTS:
// Helper function to apply color and alpha to all visuals.
void updateColorAndAlpha();
void updateHistoryLength();
private Q_SLOTS:
// Helper function to apply color and alpha to all visuals.
void updateColorAndAlpha();
void updateHistoryLength();

private:
// Function to handle an incoming ROS message.
void processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg );
private:
// Function to handle an incoming ROS message.
void processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg );

// Storage for the list of visuals par each joint intem
// Storage for the list of visuals. It is a circular buffer where
// data gets popped from the front (oldest) and pushed to the back (newest)
boost::circular_buffer<boost::shared_ptr<WrenchStampedVisual> > visuals_;
// Storage for the list of visuals par each joint intem
// Storage for the list of visuals. It is a circular buffer where
// data gets popped from the front (oldest) and pushed to the back (newest)
boost::circular_buffer<boost::shared_ptr<WrenchStampedVisual> > visuals_;

// Property objects for user-editable properties.
rviz::ColorProperty *force_color_property_, *torque_color_property_;
rviz::FloatProperty *alpha_property_, *force_scale_property_, *torque_scale_property_, *width_property_;
rviz::IntProperty *history_length_property_;
};
// Property objects for user-editable properties.
rviz::ColorProperty *force_color_property_, *torque_color_property_;
rviz::FloatProperty *alpha_property_, *force_scale_property_, *torque_scale_property_, *width_property_;
rviz::IntProperty *history_length_property_;
};
} // end namespace rviz_plugin_tutorials

#endif // WRENCHSTAMPED_DISPLAY_H
169 changes: 97 additions & 72 deletions src/rviz/default_plugin/wrench_visual.cpp
Original file line number Diff line number Diff line change
@@ -12,50 +12,69 @@
namespace rviz
{

WrenchStampedVisual::WrenchStampedVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node )
{
scene_manager_ = scene_manager;

// Ogre::SceneNode s form a tree, with each node storing the
// transform (position and orientation) of itself relative to its
// parent. Ogre does the math of combining those transforms when it
// is time to render.
//
// Here we create a node to store the pose of the WrenchStamped's header frame
// relative to the RViz fixed frame.
frame_node_ = parent_node->createChildSceneNode();

// We create the arrow object within the frame node so that we can
// set its position and direction relative to its header frame.
arrow_force_ = new rviz::Arrow( scene_manager_, frame_node_ );
arrow_torque_ = new rviz::Arrow( scene_manager_, frame_node_ );
circle_torque_ = new rviz::BillboardLine( scene_manager_, frame_node_ );
circle_arrow_torque_ = new rviz::Arrow( scene_manager_, frame_node_ );
}
WrenchStampedVisual::WrenchStampedVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node )
{
scene_manager_ = scene_manager;

// Ogre::SceneNode s form a tree, with each node storing the
// transform (position and orientation) of itself relative to its
// parent. Ogre does the math of combining those transforms when it
// is time to render.
//
// Here we create a node to store the pose of the WrenchStamped's header frame
// relative to the RViz fixed frame.
frame_node_ = parent_node->createChildSceneNode();
force_node_ = frame_node_->createChildSceneNode();
torque_node_ = frame_node_->createChildSceneNode();

// We create the arrow object within the frame node so that we can
// set its position and direction relative to its header frame.
arrow_force_ = new rviz::Arrow( scene_manager_, force_node_ );
arrow_torque_ = new rviz::Arrow( scene_manager_, torque_node_ );
circle_torque_ = new rviz::BillboardLine( scene_manager_, torque_node_ );
circle_arrow_torque_ = new rviz::Arrow( scene_manager_, torque_node_ );
}

WrenchStampedVisual::~WrenchStampedVisual()
{
// Delete the arrow to make it disappear.
delete arrow_force_;
delete arrow_torque_;
delete circle_torque_;
delete circle_arrow_torque_;

WrenchStampedVisual::~WrenchStampedVisual()
{
// Delete the arrow to make it disappear.
delete arrow_force_;
delete arrow_torque_;
delete circle_torque_;
delete circle_arrow_torque_;
// Destroy the frame node since we don't need it anymore.
scene_manager_->destroySceneNode( frame_node_ );
}

// Destroy the frame node since we don't need it anymore.
scene_manager_->destroySceneNode( frame_node_ );
}

void WrenchStampedVisual::setMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg )
{
setWrench(msg->wrench);
}

void WrenchStampedVisual::setMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg )
{
Ogre::Vector3 force(msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z);
Ogre::Vector3 torque(msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z);
double force_length = force.length() * force_scale_;
double torque_length = torque.length() * torque_scale_;
arrow_force_->setScale(Ogre::Vector3(force_length, width_, width_));
arrow_torque_->setScale(Ogre::Vector3(torque_length, width_, width_));
void WrenchStampedVisual::setWrench( const geometry_msgs::Wrench& wrench )
{
Ogre::Vector3 force(wrench.force.x, wrench.force.y, wrench.force.z);
Ogre::Vector3 torque(wrench.torque.x, wrench.torque.y, wrench.torque.z);
setWrench(force, torque);
}

void WrenchStampedVisual::setWrench( const Ogre::Vector3 &force, const Ogre::Vector3 &torque )
{
double force_length = force.length() * force_scale_;
double torque_length = torque.length() * torque_scale_;
// hide markers if they get too short
bool show_force = (force_length > width_);
bool show_torque = (torque_length > width_);
if (show_force) {
arrow_force_->setScale(Ogre::Vector3(force_length, width_, width_));
arrow_force_->setDirection(force);
}
force_node_->setVisible(show_force);

if (show_torque) {
arrow_torque_->setScale(Ogre::Vector3(torque_length, width_, width_));
arrow_torque_->setDirection(torque);
Ogre::Vector3 axis_z(0,0,1);
Ogre::Quaternion orientation = axis_z.getRotationTo(torque);
@@ -75,45 +94,51 @@ namespace rviz
circle_torque_->addPoint(orientation * point);
}
}
torque_node_->setVisible(show_torque);
}

// Position and orientation are passed through to the SceneNode.
void WrenchStampedVisual::setFramePosition( const Ogre::Vector3& position )
{
frame_node_->setPosition( position );
}
// Position and orientation are passed through to the SceneNode.
void WrenchStampedVisual::setFramePosition( const Ogre::Vector3& position )
{
frame_node_->setPosition( position );
}

void WrenchStampedVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
{
frame_node_->setOrientation( orientation );
}
void WrenchStampedVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
{
frame_node_->setOrientation( orientation );
}

// Color is passed through to the rviz object.
void WrenchStampedVisual::setForceColor( float r, float g, float b, float a )
{
arrow_force_->setColor( r, g, b, a );
}
// Color is passed through to the rviz object.
void WrenchStampedVisual::setTorqueColor( float r, float g, float b, float a )
{
arrow_torque_->setColor( r, g, b, a );
circle_torque_->setColor( r, g, b, a );
circle_arrow_torque_->setColor( r, g, b, a );
}
// Color is passed through to the rviz object.
void WrenchStampedVisual::setForceColor( float r, float g, float b, float a )
{
arrow_force_->setColor( r, g, b, a );
}
// Color is passed through to the rviz object.
void WrenchStampedVisual::setTorqueColor( float r, float g, float b, float a )
{
arrow_torque_->setColor( r, g, b, a );
circle_torque_->setColor( r, g, b, a );
circle_arrow_torque_->setColor( r, g, b, a );
}

void WrenchStampedVisual::setForceScale( float s )
{
force_scale_ = s;
}
void WrenchStampedVisual::setForceScale( float s )
{
force_scale_ = s;
}

void WrenchStampedVisual::setTorqueScale( float s )
{
torque_scale_ = s;
}
void WrenchStampedVisual::setTorqueScale( float s )
{
torque_scale_ = s;
}

void WrenchStampedVisual::setWidth( float w )
{
width_ = w;
}
void WrenchStampedVisual::setWidth( float w )
{
width_ = w;
}

} // end namespace rviz
void WrenchStampedVisual::setVisible(bool visible)
{
frame_node_->setVisible(visible);
}

} // end namespace rviz
18 changes: 18 additions & 0 deletions src/rviz/default_plugin/wrench_visual.h
Original file line number Diff line number Diff line change
@@ -3,6 +3,15 @@

#include <geometry_msgs/WrenchStamped.h>

#ifdef __GNUC__
#define RVIZ_DEPRECATED __attribute__ ((deprecated))
#elif defined(_MSC_VER)
#define RVIZ_DEPRECATED __declspec(deprecated)
#else
#pragma message("WARNING: You need to implement DEPRECATED for this compiler")
#define RVIZ_DEPRECATED
#endif

namespace Ogre
{
class Vector3;
@@ -33,8 +42,13 @@ class WrenchStampedVisual
// Destructor. Removes the visual stuff from the scene.
virtual ~WrenchStampedVisual();

// Configure the visual to show the given force and torque vectors
void setWrench( const Ogre::Vector3 &force, const Ogre::Vector3 &torque );
// Configure the visual to show the data in the message.
RVIZ_DEPRECATED
void setMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg );
// Configure the visual to show the given wrench
void setWrench( const geometry_msgs::Wrench& wrench );

// Set the pose of the coordinate frame the message refers to.
// These could be done inside setMessage(), but that would require
@@ -51,6 +65,7 @@ class WrenchStampedVisual
void setForceScale( float s );
void setTorqueScale( float s );
void setWidth( float w );
void setVisible( bool visible );

private:
// The object implementing the wrenchStamped circle
@@ -68,6 +83,9 @@ class WrenchStampedVisual
// destroy the ``frame_node_``.
Ogre::SceneManager* scene_manager_;

// allow showing/hiding of force / torque arrows
Ogre::SceneNode* force_node_;
Ogre::SceneNode* torque_node_;
};

} // end namespace rviz

0 comments on commit c6bfbb6

Please sign in to comment.