Skip to content

Commit

Permalink
Vector3 works. rviz has scale_ set to NaN for one iteration for some …
Browse files Browse the repository at this point in the history 
…reason
  • Loading branch information
@goretkin
goretkin committed Mar 9, 2015
1 parent c512f08 commit 509503d
Show file tree
Hide file tree
Showing 4 changed files with 32 additions and 76 deletions.
28 changes: 9 additions & 19 deletions src/rviz/default_plugin/vector3_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_ =
Expand Down Expand Up @@ -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 );
}
Expand All @@ -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
Expand Down Expand Up @@ -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 );

Expand Down
7 changes: 3 additions & 4 deletions src/rviz/default_plugin/vector3_display.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
#include <boost/circular_buffer.hpp>
#endif

#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <rviz/message_filter_display.h>

Expand All @@ -27,7 +26,7 @@ namespace rviz

class Vector3StampedVisual;

class Vector3StampedDisplay: public rviz::MessageFilterDisplay<geometry_msgs::WrenchStamped>
class Vector3StampedDisplay: public rviz::MessageFilterDisplay<geometry_msgs::Vector3Stamped>
{
Q_OBJECT
public:
Expand All @@ -48,15 +47,15 @@ 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
// 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 *force_color_property_, *torque_color_property_;
rviz::ColorProperty *vector_color_property_;
rviz::FloatProperty *alpha_property_, *scale_property_, *width_property_;
rviz::IntProperty *history_length_property_;
};
Expand Down
58 changes: 15 additions & 43 deletions src/rviz/default_plugin/vector3_visual.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@

#include "vector3_visual.h"

#include "cmath"

namespace rviz
{

Expand All @@ -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.
Expand All @@ -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 ) {
Expand Down
15 changes: 5 additions & 10 deletions src/rviz/default_plugin/vector3_visual.h
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
#ifndef Vector3STAMPED_VISUAL_H
#define Vector3STAMPED_VISUAL_H
#ifndef VECTOR3STAMPED_VISUAL_H
#define VECTOR3STAMPED_VISUAL_H

#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/WrenchStamped.h>

namespace Ogre
{
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down

0 comments on commit 509503d

Please sign in to comment.