From c512f087d99f76f86a39b279d5e60c3023c76db0 Mon Sep 17 00:00:00 2001 From: Gustavo N Goretkin Date: Sun, 8 Mar 2015 20:27:08 -0400 Subject: [PATCH 1/2] Infrastructure for Vector3 in place, copied from Wrench. --- plugin_description.xml | 6 + src/rviz/default_plugin/CMakeLists.txt | 3 + src/rviz/default_plugin/vector3_display.cpp | 166 ++++++++++++++++++++ src/rviz/default_plugin/vector3_display.h | 65 ++++++++ src/rviz/default_plugin/vector3_visual.cpp | 111 +++++++++++++ src/rviz/default_plugin/vector3_visual.h | 75 +++++++++ 6 files changed, 426 insertions(+) create mode 100644 src/rviz/default_plugin/vector3_display.cpp create mode 100644 src/rviz/default_plugin/vector3_display.h create mode 100644 src/rviz/default_plugin/vector3_visual.cpp create mode 100644 src/rviz/default_plugin/vector3_visual.h 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..ff707fd83b --- /dev/null +++ b/src/rviz/default_plugin/vector3_display.cpp @@ -0,0 +1,166 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "vector3_visual.h" + +#include "vector3_display.h" + +namespace rviz +{ + + Vector3StampedDisplay::Vector3StampedDisplay() + { + 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() )); + + 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 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]->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::WrenchStamped::ConstPtr& msg ) + { + #if 0 + if( !validateFloats( *msg )) + { + setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); + return; + } + #endif + + // 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 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->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..e9fde39561 --- /dev/null +++ b/src/rviz/default_plugin/vector3_display.h @@ -0,0 +1,65 @@ +#ifndef VECTOR3STAMPED_DISPLAY_H +#define VECTOR3STAMPED_DISPLAY_H + +#ifndef Q_MOC_RUN +#include +#endif + +#include +#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::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 > visuals_; + + // Property objects for user-editable properties. + rviz::ColorProperty *force_color_property_, *torque_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..1dde7886b6 --- /dev/null +++ b/src/rviz/default_plugin/vector3_visual.cpp @@ -0,0 +1,111 @@ +#include +#include +#include + +#include +#include + +#include + +#include "vector3_visual.h" + +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_force_ = new rviz::Arrow( scene_manager_, frame_node_ ); + arrow_torque_ = new rviz::Arrow( scene_manager_, frame_node_ ); + circle_torque_ = new rviz::BillboardLine( scene_manager_, frame_node_ ); + circle_arrow_torque_ = new rviz::Arrow( scene_manager_, frame_node_ ); + } + + Vector3StampedVisual::~Vector3StampedVisual() + { + // Delete the arrow to make it disappear. + delete arrow_force_; + delete arrow_torque_; + delete circle_torque_; + delete circle_arrow_torque_; + + // Destroy the frame node since we don't need it anymore. + scene_manager_->destroySceneNode( frame_node_ ); + } + + + void Vector3StampedVisual::setMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg ) + { + Ogre::Vector3 force(msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z); + Ogre::Vector3 torque(msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z); + double force_length = force.length() * scale_; + double torque_length = torque.length() * scale_; + arrow_force_->setScale(Ogre::Vector3(force_length, width_, width_)); + arrow_torque_->setScale(Ogre::Vector3(torque_length, width_, width_)); + + arrow_force_->setDirection(force); + arrow_torque_->setDirection(torque); + Ogre::Vector3 axis_z(0,0,1); + Ogre::Quaternion orientation(axis_z.angleBetween(torque), axis_z.crossProduct(torque.normalisedCopy())); + if ( std::isnan(orientation.x) || + std::isnan(orientation.y) || + std::isnan(orientation.z) ) orientation = Ogre::Quaternion::IDENTITY; + //circle_arrow_torque_->setScale(Ogre::Vector3(width_, width_, 0.05)); + circle_arrow_torque_->set(0, width_*0.1, width_*0.1*1.0, width_*0.1*2.0); + circle_arrow_torque_->setDirection(orientation * Ogre::Vector3(0,1,0)); + circle_arrow_torque_->setPosition(orientation * Ogre::Vector3(torque_length/4, 0, torque_length/2)); + circle_torque_->clear(); + circle_torque_->setLineWidth(width_*0.05); + for (int i = 4; i <= 32; i++) { + Ogre::Vector3 point = Ogre::Vector3((torque_length/4)*cos(i*2*M_PI/32), + (torque_length/4)*sin(i*2*M_PI/32), + torque_length/2); + circle_torque_->addPoint(orientation * point); + } + } + + // 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::setForceColor( float r, float g, float b, float a ) + { + arrow_force_->setColor( r, g, b, a ); + } + // Color is passed through to the rviz object. + void Vector3StampedVisual::setTorqueColor( float r, float g, float b, float a ) + { + arrow_torque_->setColor( r, g, b, a ); + circle_torque_->setColor( r, g, b, a ); + circle_arrow_torque_->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..9773f71663 --- /dev/null +++ b/src/rviz/default_plugin/vector3_visual.h @@ -0,0 +1,75 @@ +#ifndef Vector3STAMPED_VISUAL_H +#define Vector3STAMPED_VISUAL_H + +#include +#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::WrenchStamped::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 setForceColor( float r, float g, float b, float a ); + void setTorqueColor( 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_force_; + rviz::Arrow* arrow_torque_; + rviz::BillboardLine* circle_torque_; + rviz::Arrow* circle_arrow_torque_; + 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 From 509503d4753851e181081a9256d7ef947d8fd77d Mon Sep 17 00:00:00 2001 From: Gustavo N Goretkin Date: Sun, 8 Mar 2015 23:18:18 -0400 Subject: [PATCH 2/2] Vector3 works. rviz has scale_ set to NaN for one iteration for some reason --- src/rviz/default_plugin/vector3_display.cpp | 28 ++++------ src/rviz/default_plugin/vector3_display.h | 7 ++- src/rviz/default_plugin/vector3_visual.cpp | 58 ++++++--------------- src/rviz/default_plugin/vector3_visual.h | 15 ++---- 4 files changed, 32 insertions(+), 76 deletions(-) diff --git a/src/rviz/default_plugin/vector3_display.cpp b/src/rviz/default_plugin/vector3_display.cpp index ff707fd83b..9f25e853bf 100644 --- a/src/rviz/default_plugin/vector3_display.cpp +++ b/src/rviz/default_plugin/vector3_display.cpp @@ -20,14 +20,9 @@ namespace rviz Vector3StampedDisplay::Vector3StampedDisplay() { - 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.", + vector_color_property_ = + new rviz::ColorProperty( "Vector Color", QColor( 204, 51, 51 ), + "Color to draw the arrow.", this, SLOT( updateColorAndAlpha() )); alpha_property_ = @@ -77,13 +72,11 @@ namespace rviz float alpha = alpha_property_->getFloat(); float scale = scale_property_->getFloat(); float width = width_property_->getFloat(); - Ogre::ColourValue force_color = force_color_property_->getOgreColor(); - Ogre::ColourValue torque_color = torque_color_property_->getOgreColor(); + Ogre::ColourValue vector_color = vector_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]->setVectorColor( vector_color.r, vector_color.g, vector_color.b, alpha ); visuals_[i]->setScale( scale ); visuals_[i]->setWidth( width ); } @@ -101,15 +94,13 @@ namespace rviz } // This is our callback to handle an incoming message. - void Vector3StampedDisplay::processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg ) + void Vector3StampedDisplay::processMessage( const geometry_msgs::Vector3Stamped::ConstPtr& msg ) { - #if 0 if( !validateFloats( *msg )) { setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); return; } - #endif // 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 @@ -144,10 +135,9 @@ namespace rviz float alpha = alpha_property_->getFloat(); float scale = 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 ); + 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 ); diff --git a/src/rviz/default_plugin/vector3_display.h b/src/rviz/default_plugin/vector3_display.h index e9fde39561..ea1ea874f3 100644 --- a/src/rviz/default_plugin/vector3_display.h +++ b/src/rviz/default_plugin/vector3_display.h @@ -5,7 +5,6 @@ #include #endif -#include #include #include @@ -27,7 +26,7 @@ namespace rviz class Vector3StampedVisual; - class Vector3StampedDisplay: public rviz::MessageFilterDisplay + class Vector3StampedDisplay: public rviz::MessageFilterDisplay { Q_OBJECT public: @@ -48,7 +47,7 @@ namespace rviz private: // Function to handle an incoming ROS message. - void processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg ); + 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 @@ -56,7 +55,7 @@ namespace rviz boost::circular_buffer > visuals_; // Property objects for user-editable properties. - rviz::ColorProperty *force_color_property_, *torque_color_property_; + rviz::ColorProperty *vector_color_property_; rviz::FloatProperty *alpha_property_, *scale_property_, *width_property_; rviz::IntProperty *history_length_property_; }; diff --git a/src/rviz/default_plugin/vector3_visual.cpp b/src/rviz/default_plugin/vector3_visual.cpp index 1dde7886b6..05f0ec63ee 100644 --- a/src/rviz/default_plugin/vector3_visual.cpp +++ b/src/rviz/default_plugin/vector3_visual.cpp @@ -9,6 +9,8 @@ #include "vector3_visual.h" +#include "cmath" + namespace rviz { @@ -27,53 +29,30 @@ namespace rviz // We create the arrow object within the frame node so that we can // set its position and direction relative to its header frame. - arrow_force_ = new rviz::Arrow( scene_manager_, frame_node_ ); - arrow_torque_ = new rviz::Arrow( scene_manager_, frame_node_ ); - circle_torque_ = new rviz::BillboardLine( scene_manager_, frame_node_ ); - circle_arrow_torque_ = new rviz::Arrow( scene_manager_, frame_node_ ); + arrow_vector_ = new rviz::Arrow( scene_manager_, frame_node_ ); } Vector3StampedVisual::~Vector3StampedVisual() { // Delete the arrow to make it disappear. - delete arrow_force_; - delete arrow_torque_; - delete circle_torque_; - delete circle_arrow_torque_; + 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::WrenchStamped::ConstPtr& msg ) + void Vector3StampedVisual::setMessage( const geometry_msgs::Vector3Stamped::ConstPtr& msg ) { - Ogre::Vector3 force(msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z); - Ogre::Vector3 torque(msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z); - double force_length = force.length() * scale_; - double torque_length = torque.length() * scale_; - arrow_force_->setScale(Ogre::Vector3(force_length, width_, width_)); - arrow_torque_->setScale(Ogre::Vector3(torque_length, width_, width_)); - - arrow_force_->setDirection(force); - arrow_torque_->setDirection(torque); - Ogre::Vector3 axis_z(0,0,1); - Ogre::Quaternion orientation(axis_z.angleBetween(torque), axis_z.crossProduct(torque.normalisedCopy())); - if ( std::isnan(orientation.x) || - std::isnan(orientation.y) || - std::isnan(orientation.z) ) orientation = Ogre::Quaternion::IDENTITY; - //circle_arrow_torque_->setScale(Ogre::Vector3(width_, width_, 0.05)); - circle_arrow_torque_->set(0, width_*0.1, width_*0.1*1.0, width_*0.1*2.0); - circle_arrow_torque_->setDirection(orientation * Ogre::Vector3(0,1,0)); - circle_arrow_torque_->setPosition(orientation * Ogre::Vector3(torque_length/4, 0, torque_length/2)); - circle_torque_->clear(); - circle_torque_->setLineWidth(width_*0.05); - for (int i = 4; i <= 32; i++) { - Ogre::Vector3 point = Ogre::Vector3((torque_length/4)*cos(i*2*M_PI/32), - (torque_length/4)*sin(i*2*M_PI/32), - torque_length/2); - circle_torque_->addPoint(orientation * point); + 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. @@ -88,16 +67,9 @@ namespace rviz } // Color is passed through to the rviz object. - void Vector3StampedVisual::setForceColor( float r, float g, float b, float a ) - { - arrow_force_->setColor( r, g, b, a ); - } - // Color is passed through to the rviz object. - void Vector3StampedVisual::setTorqueColor( float r, float g, float b, float a ) + void Vector3StampedVisual::setVectorColor( float r, float g, float b, float a ) { - arrow_torque_->setColor( r, g, b, a ); - circle_torque_->setColor( r, g, b, a ); - circle_arrow_torque_->setColor( r, g, b, a ); + arrow_vector_->setColor( r, g, b, a ); } void Vector3StampedVisual::setScale( float s ) { diff --git a/src/rviz/default_plugin/vector3_visual.h b/src/rviz/default_plugin/vector3_visual.h index 9773f71663..f44259256e 100644 --- a/src/rviz/default_plugin/vector3_visual.h +++ b/src/rviz/default_plugin/vector3_visual.h @@ -1,8 +1,7 @@ -#ifndef Vector3STAMPED_VISUAL_H -#define Vector3STAMPED_VISUAL_H +#ifndef VECTOR3STAMPED_VISUAL_H +#define VECTOR3STAMPED_VISUAL_H #include -#include namespace Ogre { @@ -35,7 +34,7 @@ class Vector3StampedVisual virtual ~Vector3StampedVisual(); // Configure the visual to show the data in the message. - void setMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg ); + 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 @@ -47,17 +46,13 @@ class Vector3StampedVisual // Set the color and alpha of the visual, which are user-editable // parameters and therefore don't come from the WrenchStamped message. - void setForceColor( float r, float g, float b, float a ); - void setTorqueColor( float r, float g, float b, float a ); + 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_force_; - rviz::Arrow* arrow_torque_; - rviz::BillboardLine* circle_torque_; - rviz::Arrow* circle_arrow_torque_; + rviz::Arrow* arrow_vector_; float scale_, width_; // A SceneNode whose pose is set to match the coordinate frame of