hiding small wrenches should be optional (#1196)

* added a boolean property to the wrench visualization to make hiding small forces/torques optional

* update doc text

* whitespace fixup

src/rviz/default_plugin/wrench_display.cpp

@@ -23,39 +23,44 @@ WrenchStampedDisplay::WrenchStampedDisplay()
     force_color_property_ =
             new rviz::ColorProperty( "Force Color", QColor( 204, 51, 51 ),
                                      "Color to draw the force arrows.",
-                                     this, SLOT( updateColorAndAlpha() ));
+                                     this, SLOT( updateProperties() ));
 
     torque_color_property_ =
             new rviz::ColorProperty( "Torque Color", QColor( 204, 204, 51),
                                      "Color to draw the torque arrows.",
-                                     this, SLOT( updateColorAndAlpha() ));
+                                     this, SLOT( updateProperties() ));
 
     alpha_property_ =
             new rviz::FloatProperty( "Alpha", 1.0,
                                      "0 is fully transparent, 1.0 is fully opaque.",
-                                     this, SLOT( updateColorAndAlpha() ));
+                                     this, SLOT( updateProperties() ));
 
     force_scale_property_ =
             new rviz::FloatProperty( "Force Arrow Scale", 2.0,
                                      "force arrow scale",
-                                     this, SLOT( updateColorAndAlpha() ));
+                                     this, SLOT( updateProperties() ));
 
     torque_scale_property_ =
             new rviz::FloatProperty( "Torque Arrow Scale", 2.0,
                                      "torque arrow scale",
-                                     this, SLOT( updateColorAndAlpha() ));
+                                     this, SLOT( updateProperties() ));
 
     width_property_ =
             new rviz::FloatProperty( "Arrow Width", 0.5,
                                      "arrow width",
-                                     this, SLOT( updateColorAndAlpha() ));
+                                     this, SLOT( updateProperties() ));
 
 
     history_length_property_ =
             new rviz::IntProperty( "History Length", 1,
                                    "Number of prior measurements to display.",
                                    this, SLOT( updateHistoryLength() ));
 
+    hide_small_values_property_ =
+            new rviz::BoolProperty( "Hide Small Values", true,
+                                    "Hide small values",
+                                    this, SLOT( updateProperties() ));
+
     history_length_property_->setMin( 1 );
     history_length_property_->setMax( 100000 );
 }
@@ -77,12 +82,13 @@ void WrenchStampedDisplay::reset()
     visuals_.clear();
 }
 
-void WrenchStampedDisplay::updateColorAndAlpha()
+void WrenchStampedDisplay::updateProperties()
 {
     float alpha = alpha_property_->getFloat();
     float force_scale = force_scale_property_->getFloat();
     float torque_scale = torque_scale_property_->getFloat();
     float width = width_property_->getFloat();
+    bool hide_small_values = hide_small_values_property_->getBool();
     Ogre::ColourValue force_color = force_color_property_->getOgreColor();
     Ogre::ColourValue torque_color = torque_color_property_->getOgreColor();
 
@@ -93,6 +99,7 @@ void WrenchStampedDisplay::updateColorAndAlpha()
         visuals_[i]->setForceScale( force_scale );
         visuals_[i]->setTorqueScale( torque_scale );
         visuals_[i]->setWidth( width );
+        visuals_[i]->setHideSmallValues( hide_small_values );
     }
 }
 
@@ -102,6 +109,7 @@ void WrenchStampedDisplay::updateHistoryLength()
     visuals_.rset_capacity(history_length_property_->getInt());
 }
 
+
 bool validateFloats( const geometry_msgs::WrenchStamped& msg )
 {
     return rviz::validateFloats(msg.wrench.force) && rviz::validateFloats(msg.wrench.torque) ;

src/rviz/default_plugin/wrench_display.h

@@ -41,8 +41,8 @@ class WrenchStampedDisplay: public rviz::MessageFilterDisplay<geometry_msgs::Wre
     virtual void reset();
 
 private Q_SLOTS:
-    // Helper function to apply color and alpha to all visuals.
-    void updateColorAndAlpha();
+    // Helper function to properties for all visuals.
+    void updateProperties();
     void updateHistoryLength();
 
 private:
@@ -58,6 +58,7 @@ private Q_SLOTS:
     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_;
+    rviz::BoolProperty *hide_small_values_property_;
 };
 } // end namespace rviz_plugin_tutorials
 

src/rviz/default_plugin/wrench_visual.cpp

@@ -59,9 +59,11 @@ void WrenchVisual::setWrench( const Ogre::Vector3 &force, const Ogre::Vector3 &t
 {
     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_);
+    // hide markers if they get too short and hide_small_values_ is activated
+    // "too short" is defined as "force_length > width_"
+    bool show_force = (force_length > width_)   || !hide_small_values_;
+    bool show_torque = (torque_length > width_) || !hide_small_values_;
+
     if (show_force) {
         arrow_force_->setScale(Ogre::Vector3(force_length, width_, width_));
         arrow_force_->setDirection(force);
@@ -131,6 +133,12 @@ void  WrenchVisual::setWidth( float w )
     width_ = w;
 }
 
+void  WrenchVisual::setHideSmallValues( bool h )
+{
+    hide_small_values_ = h;
+}
+
+
 void WrenchVisual::setVisible(bool visible)
 {
     frame_node_->setVisible(visible);

src/rviz/default_plugin/wrench_visual.h

@@ -53,6 +53,7 @@ class WrenchVisual
     void setForceScale( float s );
     void setTorqueScale( float s );
     void setWidth( float w );
+    void setHideSmallValues( bool h );
     void setVisible( bool visible );
 
 private:
@@ -62,6 +63,7 @@ class WrenchVisual
     rviz::BillboardLine* circle_torque_;
     rviz::Arrow* circle_arrow_torque_;
     float force_scale_, torque_scale_, width_;
+    bool hide_small_values_;
 
     // A SceneNode whose pose is set to match the coordinate frame of
     // the WrenchStamped message header.