wrench_display.cpp

Go to the documentation of this file.

#include <OgreSceneNode.h>
#include <OgreSceneManager.h>

#include <rviz/visualization_manager.h>
#include <rviz/frame_manager.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/properties/parse_color.h>
#include <rviz/validate_floats.h>

#include <boost/foreach.hpp>

#include "wrench_visual.h"

#include "wrench_display.h"

namespace rviz
{

    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),
                                     "Color to draw the torque arrows.",
                                     this, SLOT( updateColorAndAlpha() ));

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

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

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

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


        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 );
    }

    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();
    }

    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());
    }

    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 ))
            {
                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);
    }

} // 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 )