generalize WrenchVisual class (#982)

* 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

Author: rhaschke

src/rviz/default_plugin/wrench_display.cpp

@@ -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 )
-
-
-

src/rviz/default_plugin/wrench_display.h

@@ -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