diff --git a/plugin_description.xml b/plugin_description.xml index 9cde0ac991..d93ffbdc00 100644 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -150,6 +150,12 @@ + + + Displays from geometry_msgs/Vector3Stamped message + + + Allows you to click on a point and publish it as a PointStamped message. diff --git a/src/rviz/default_plugin/CMakeLists.txt b/src/rviz/default_plugin/CMakeLists.txt index 25fc23f57f..f4e7d35fbf 100644 --- a/src/rviz/default_plugin/CMakeLists.txt +++ b/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 diff --git a/src/rviz/default_plugin/vector3_display.cpp b/src/rviz/default_plugin/vector3_display.cpp new file mode 100644 index 0000000000..9f25e853bf --- /dev/null +++ b/src/rviz/default_plugin/vector3_display.cpp @@ -0,0 +1,156 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#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 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_EXPORT_CLASS( rviz::Vector3StampedDisplay, rviz::Display ) + + + diff --git a/src/rviz/default_plugin/vector3_display.h b/src/rviz/default_plugin/vector3_display.h new file mode 100644 index 0000000000..ea1ea874f3 --- /dev/null +++ b/src/rviz/default_plugin/vector3_display.h @@ -0,0 +1,64 @@ +#ifndef VECTOR3STAMPED_DISPLAY_H +#define VECTOR3STAMPED_DISPLAY_H + +#ifndef Q_MOC_RUN +#include +#endif + +#include +#include + +namespace Ogre +{ + class SceneNode; +} + +namespace rviz +{ + class ColorProperty; + class ROSTopicStringProperty; + class FloatProperty; + class IntProperty; +} + +namespace rviz +{ + + class Vector3StampedVisual; + + class Vector3StampedDisplay: public rviz::MessageFilterDisplay + { + 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 > 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 diff --git a/src/rviz/default_plugin/vector3_visual.cpp b/src/rviz/default_plugin/vector3_visual.cpp new file mode 100644 index 0000000000..05f0ec63ee --- /dev/null +++ b/src/rviz/default_plugin/vector3_visual.cpp @@ -0,0 +1,83 @@ +#include +#include +#include + +#include +#include + +#include + +#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 + diff --git a/src/rviz/default_plugin/vector3_visual.h b/src/rviz/default_plugin/vector3_visual.h new file mode 100644 index 0000000000..f44259256e --- /dev/null +++ b/src/rviz/default_plugin/vector3_visual.h @@ -0,0 +1,70 @@ +#ifndef VECTOR3STAMPED_VISUAL_H +#define VECTOR3STAMPED_VISUAL_H + +#include + +namespace Ogre +{ + class Vector3; + class Quaternion; +} + +namespace rviz +{ + class Arrow; + class BillboardLine; +} + +namespace rviz +{ + + +// Each instance of WrenchStampedVisual represents the visualization of a single +// sensor_msgs::WrenchStamped message. Currently it just shows an arrow with +// the direction and magnitude of the acceleration vector, but could +// easily be expanded to include more of the message data. +class Vector3StampedVisual +{ +public: + // Constructor. Creates the visual stuff and puts it into the + // scene, but in an unconfigured state. + Vector3StampedVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ); + + // Destructor. Removes the visual stuff from the scene. + virtual ~Vector3StampedVisual(); + + // Configure the visual to show the data in the message. + void setMessage( const geometry_msgs::Vector3Stamped::ConstPtr& msg ); + + // Set the pose of the coordinate frame the message refers to. + // These could be done inside setMessage(), but that would require + // calls to FrameManager and error handling inside setMessage(), + // which doesn't seem as clean. This way WrenchStampedVisual is only + // responsible for visualization. + void setFramePosition( const Ogre::Vector3& position ); + void setFrameOrientation( const Ogre::Quaternion& orientation ); + + // Set the color and alpha of the visual, which are user-editable + // parameters and therefore don't come from the WrenchStamped message. + void setVectorColor( float r, float g, float b, float a ); + void setScale( float s ); + void setWidth( float w ); + +private: + // The object implementing the wrenchStamped circle + rviz::Arrow* arrow_vector_; + float scale_, width_; + + // A SceneNode whose pose is set to match the coordinate frame of + // the WrenchStamped message header. + Ogre::SceneNode* frame_node_; + + // The SceneManager, kept here only so the destructor can ask it to + // destroy the ``frame_node_``. + Ogre::SceneManager* scene_manager_; + +}; + +} // end namespace rviz + +#endif // VECTOR3STAMPED_VISUAL_H