[WIP] Display Type Vector3Stamped

plugin_description.xml

@@ -150,6 +150,12 @@
     </description>
   </class>
 
+  <class name="rviz/Vector3Stamped" type="rviz::Vector3StampedDisplay" base_class_type="rviz::Display">
+    <description>
+      Displays from geometry_msgs/Vector3Stamped message
+    </description>
+  </class>
+
   <class name="rviz/PublishPoint" type="rviz::PointTool" base_class_type="rviz::Tool">
     <description>
       Allows you to click on a point and publish it as a PointStamped message.

src/rviz/default_plugin/CMakeLists.txt

@@ -33,6 +33,7 @@ qt4_wrap_cpp(MOC_FILES
   tools/initial_pose_tool.h
   tools/interaction_tool.h
   tools/point_tool.h
+  vector3_display.h
   view_controllers/orbit_view_controller.h
   view_controllers/xy_orbit_view_controller.h
   view_controllers/fixed_orientation_ortho_view_controller.h
@@ -91,6 +92,8 @@ set(SOURCE_FILES
   tools/initial_pose_tool.cpp
   tools/selection_tool.cpp
   tools/interaction_tool.cpp
+  vector3_display.cpp
+  vector3_visual.cpp
   view_controllers/orbit_view_controller.cpp
   view_controllers/xy_orbit_view_controller.cpp
   view_controllers/fixed_orientation_ortho_view_controller.cpp

src/rviz/default_plugin/vector3_display.cpp

@@ -0,0 +1,156 @@
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/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 "vector3_visual.h"
+
+#include "vector3_display.h"
+
+namespace rviz
+{
+
+    Vector3StampedDisplay::Vector3StampedDisplay()
+    {
+	vector_color_property_ =
+	    new rviz::ColorProperty( "Vector Color", QColor( 204, 51, 51 ),
+                                     "Color to draw the arrow.",
+                                     this, SLOT( updateColorAndAlpha() ));
+
+	alpha_property_ =
+            new rviz::FloatProperty( "Alpha", 1.0,
+                                     "0 is fully transparent, 1.0 is fully opaque.",
+                                     this, SLOT( updateColorAndAlpha() ));
+
+	scale_property_ =
+            new rviz::FloatProperty( "Arrow Scale", 2.0,
+                                     "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 Vector3StampedDisplay::onInitialize()
+    {
+        MFDClass::onInitialize();
+	updateHistoryLength( );
+    }
+
+    Vector3StampedDisplay::~Vector3StampedDisplay()
+    {
+    }
+
+    // Override rviz::Display's reset() function to add a call to clear().
+    void Vector3StampedDisplay::reset()
+    {
+        MFDClass::reset();
+	visuals_.clear();
+    }
+
+    void Vector3StampedDisplay::updateColorAndAlpha()
+    {
+        float alpha = alpha_property_->getFloat();
+        float scale = scale_property_->getFloat();
+        float width = width_property_->getFloat();
+        Ogre::ColourValue vector_color = vector_color_property_->getOgreColor();
+
+        for( size_t i = 0; i < visuals_.size(); i++ )
+	{
+            visuals_[i]->setVectorColor( vector_color.r, vector_color.g, vector_color.b, alpha );
+            visuals_[i]->setScale( scale );
+            visuals_[i]->setWidth( width );
+	}
+    }
+
+    // Set the number of past visuals to show.
+    void Vector3StampedDisplay::updateHistoryLength()
+    {
+        visuals_.rset_capacity(history_length_property_->getInt());
+    }
+
+    bool validateFloats( const geometry_msgs::Vector3Stamped& msg )
+    {
+        return rviz::validateFloats(msg.vector) ;
+    }
+
+    // This is our callback to handle an incoming message.
+    void Vector3StampedDisplay::processMessage( const geometry_msgs::Vector3Stamped::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;
+	  }
+
+        // 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<Vector3StampedVisual> visual;
+        if( visuals_.full() )
+            {
+                visual = visuals_.front();
+            }
+        else
+            {
+                visual.reset(new Vector3StampedVisual( 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 scale = scale_property_->getFloat();
+        float width = width_property_->getFloat();
+        Ogre::ColourValue vector_color = vector_color_property_->getOgreColor();
+	visual->setVectorColor( vector_color.r, vector_color.g, vector_color.b, alpha );
+
+        visual->setScale( 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::Vector3StampedDisplay, rviz::Display )
+
+
+

src/rviz/default_plugin/vector3_display.h

@@ -0,0 +1,64 @@
+#ifndef VECTOR3STAMPED_DISPLAY_H
+#define VECTOR3STAMPED_DISPLAY_H
+
+#ifndef Q_MOC_RUN
+#include <boost/circular_buffer.hpp>
+#endif
+
+#include <geometry_msgs/Vector3Stamped.h>
+#include <rviz/message_filter_display.h>
+
+namespace Ogre
+{
+    class SceneNode;
+}
+
+namespace rviz
+{
+    class ColorProperty;
+    class ROSTopicStringProperty;
+    class FloatProperty;
+    class IntProperty;
+}
+
+namespace rviz
+{
+
+    class Vector3StampedVisual;
+
+    class Vector3StampedDisplay: public rviz::MessageFilterDisplay<geometry_msgs::Vector3Stamped>
+    {
+    Q_OBJECT
+    public:
+	// Constructor.  pluginlib::ClassLoader creates instances by calling
+	// the default constructor, so make sure you have one.
+	Vector3StampedDisplay();
+	virtual ~Vector3StampedDisplay();
+
+    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:
+	// Function to handle an incoming ROS message.
+	void processMessage( const geometry_msgs::Vector3Stamped::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<Vector3StampedVisual> > visuals_;
+
+	// Property objects for user-editable properties.
+        rviz::ColorProperty *vector_color_property_;
+        rviz::FloatProperty *alpha_property_, *scale_property_, *width_property_;
+	rviz::IntProperty *history_length_property_;
+    };
+} // end namespace rviz_plugin_tutorials
+
+#endif // VECTOR3STAMPED_DISPLAY_H

src/rviz/default_plugin/vector3_visual.cpp

@@ -0,0 +1,83 @@
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+
+#include <rviz/ogre_helpers/arrow.h>
+#include <rviz/ogre_helpers/billboard_line.h>
+
+#include <ros/ros.h>
+
+#include "vector3_visual.h"
+
+#include "cmath"
+
+namespace rviz
+{
+
+    Vector3StampedVisual::Vector3StampedVisual( 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_vector_ = new rviz::Arrow( scene_manager_, frame_node_ );
+    }
+
+    Vector3StampedVisual::~Vector3StampedVisual()
+    {
+	// Delete the arrow to make it disappear.
+	delete arrow_vector_;
+
+	// Destroy the frame node since we don't need it anymore.
+	scene_manager_->destroySceneNode( frame_node_ );
+    }
+
+
+    void Vector3StampedVisual::setMessage( const geometry_msgs::Vector3Stamped::ConstPtr& msg )
+    {
+        Ogre::Vector3 vector(msg->vector.x, msg->vector.y, msg->vector.z);
+        double vector_length = vector.length() * scale_;
+        if (isnan(scale_))  //if you pass a NaN into setScale below, an Ogre assertion triggers and rviz crashes.
+        {
+            ROS_ERROR("scale_ is nan");
+            vector_length = 0.0;
+        }
+    	arrow_vector_->setScale(Ogre::Vector3(vector_length, width_, width_)); 
+        arrow_vector_->setDirection(vector);
+    }
+
+    // Position and orientation are passed through to the SceneNode.
+    void Vector3StampedVisual::setFramePosition( const Ogre::Vector3& position )
+    {
+	frame_node_->setPosition( position );
+    }
+
+    void Vector3StampedVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
+    {
+	frame_node_->setOrientation( orientation );
+    }
+
+    // Color is passed through to the rviz object.
+    void Vector3StampedVisual::setVectorColor( float r, float g, float b, float a )
+    {
+	arrow_vector_->setColor( r, g, b, a );
+    }
+
+    void  Vector3StampedVisual::setScale( float s ) {
+      scale_ = s;
+    }
+    void  Vector3StampedVisual::setWidth( float w ) {
+      width_ = w;
+    }
+
+} // end namespace rviz
+