ImuDisplay¶
The RViz plugin API and library API are preliminary in Fuerte. We welcome feedback about how to make them more powerful and easier to program with. We expect the APIs to change (possibly significantly) between Fuerte and Groovy.
Overview¶
This tutorial shows how to write a simple Display plugin for RViz.
RViz does not currently have a way to display sensor_msgs/Imu messages directly. The code in this tutorial implements a subclass of rviz::Display to do so.
The source code for this tutorial is in the rviz_plugin_tutorials package. You can check out the source directly or (if you use Ubuntu) you can just apt-get install the pre-compiled Debian package like so:
sudo apt-get install ros-fuerte-visualization-tutorials
Here is what the new ImuDisplay output looks like, showing a sequence of sensor_msgs/Imu messages from the test script:
The Plugin Code¶
The code for ImuDisplay is in these files: src/imu_display.h, src/imu_display.cpp, src/imu_visual.h, and src/imu_visual.cpp.
imu_display.h¶
The full text of imu_display.h is here: src/imu_display.h
Here we declare our new subclass of rviz::Display. Every display which can be listed in the “Displays” panel is a subclass of rviz::Display.
ImuDisplay will show a 3D arrow showing the direction and magnitude of the IMU acceleration vector. The base of the arrow will be at the frame listed in the header of the Imu message, and the direction of the arrow will be relative to the orientation of that frame. It will also optionally show a history of recent acceleration vectors, which will be stored in a circular buffer.
The ImuDisplay class itself just implements the circular buffer, editable parameters, and Display subclass machinery. The visuals themselves are represented by a separate class, ImuVisual. The idiom for the visuals is that when the objects exist, they appear in the scene, and when they are deleted, they disappear.
class ImuDisplay: public rviz::Display
{
public:
Constructor. pluginlib::ClassLoader creates instances by calling the default constructor, so make sure you have one.
ImuDisplay();
virtual ~ImuDisplay();
Overrides of public virtual functions from the Display class.
virtual void onInitialize();
virtual void fixedFrameChanged();
virtual void reset();
virtual void createProperties();
Setter and getter functions for user-editable properties.
void setTopic(const std::string& topic);
const std::string& getTopic() { return topic_; }
void setColor( const rviz::Color& color );
const rviz::Color& getColor() { return color_; }
void setAlpha( float alpha );
float getAlpha() { return alpha_; }
void setHistoryLength( int history_length );
int getHistoryLength() const { return history_length_; }
Overrides of protected virtual functions from Display. As much as possible, when Displays are not enabled, they should not be subscribed to incoming data and should not show anything in the 3D view. These functions are where these connections are made and broken.
protected:
virtual void onEnable();
virtual void onDisable();
Function to handle an incoming ROS message.
private:
void incomingMessage( const sensor_msgs::Imu::ConstPtr& msg );
Internal helpers which do the work of subscribing and unsubscribing from the ROS topic.
void subscribe();
void unsubscribe();
A helper to clear this display back to the initial state.
void clear();
Helper function to apply color and alpha to all visuals.
void updateColorAndAlpha();
Storage for the list of visuals. This display supports an adjustable history length, so we need one visual per history item.
std::vector<ImuVisual*> visuals_;
A node in the Ogre scene tree to be the parent of all our visuals.
Ogre::SceneNode* scene_node_;
Data input: Subscriber and tf message filter.
message_filters::Subscriber<sensor_msgs::Imu> sub_;
tf::MessageFilter<sensor_msgs::Imu>* tf_filter_;
int messages_received_;
User-editable property variables.
rviz::Color color_;
std::string topic_;
float alpha_;
int history_length_;
Property objects for user-editable properties.
rviz::ColorPropertyWPtr color_property_;
rviz::ROSTopicStringPropertyWPtr topic_property_;
rviz::FloatPropertyWPtr alpha_property_;
rviz::IntPropertyWPtr history_length_property_;
};
imu_display.cpp¶
The full text of imu_display.cpp is here: src/imu_display.cpp
The constructor must have no arguments, so we can’t give the constructor the parameters it needs to fully initialize.
ImuDisplay::ImuDisplay()
: Display()
, scene_node_( NULL )
, messages_received_( 0 )
, color_( .8, .2, .8 ) // Default color is bright purple.
, alpha_( 1.0 ) // Default alpha is completely opaque.
{
}
After the parent rviz::Display::initialize() does its own setup, it calls the subclass’s onInitialize() function. This is where we instantiate all the workings of the class.
void ImuDisplay::onInitialize()
{
Make an Ogre::SceneNode to contain all our visuals.
scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
Set the default history length and resize the visuals_ array.
setHistoryLength( 1 );
A tf::MessageFilter listens to ROS messages and calls our callback with them when they can be matched up with valid tf transform data.
tf_filter_ =
new tf::MessageFilter<sensor_msgs::Imu>( *vis_manager_->getTFClient(),
"", 100, update_nh_ );
tf_filter_->connectInput( sub_ );
tf_filter_->registerCallback( boost::bind( &ImuDisplay::incomingMessage,
this, _1 ));
FrameManager has some built-in functions to set the status of a Display based on callbacks from a tf::MessageFilter. These work fine for this simple display.
vis_manager_->getFrameManager()
->registerFilterForTransformStatusCheck( tf_filter_, this );
}
ImuDisplay::~ImuDisplay()
{
unsubscribe();
clear();
for( size_t i = 0; i < visuals_.size(); i++ )
{
delete visuals_[ i ];
}
delete tf_filter_;
}
Clear the visuals by deleting their objects.
void ImuDisplay::clear()
{
for( size_t i = 0; i < visuals_.size(); i++ )
{
delete visuals_[ i ];
visuals_[ i ] = NULL;
}
tf_filter_->clear();
messages_received_ = 0;
setStatus( rviz::status_levels::Warn, "Topic", "No messages received" );
}
void ImuDisplay::setTopic( const std::string& topic )
{
unsubscribe();
clear();
topic_ = topic;
subscribe();
Broadcast the fact that the variable has changed.
propertyChanged( topic_property_ );
Make sure rviz renders the next time it gets a chance.
causeRender();
}
void ImuDisplay::setColor( const rviz::Color& color )
{
color_ = color;
propertyChanged( color_property_ );
updateColorAndAlpha();
causeRender();
}
void ImuDisplay::setAlpha( float alpha )
{
alpha_ = alpha;
propertyChanged( alpha_property_ );
updateColorAndAlpha();
causeRender();
}
Set the current color and alpha values for each visual.
void ImuDisplay::updateColorAndAlpha()
{
for( size_t i = 0; i < visuals_.size(); i++ )
{
if( visuals_[ i ] )
{
visuals_[ i ]->setColor( color_.r_, color_.g_, color_.b_, alpha_ );
}
}
}
Set the number of past visuals to show.
void ImuDisplay::setHistoryLength( int length )
{
Don’t let people enter invalid values.
if( length < 1 )
{
length = 1;
}
If the length is not changing, we don’t need to do anything.
if( history_length_ == length )
{
return;
}
Set the actual variable.
history_length_ = length;
propertyChanged( history_length_property_ );
Create a new array of visual pointers, all NULL.
std::vector<ImuVisual*> new_visuals( history_length_, (ImuVisual*)0 );
Copy the contents from the old array to the new. (Number to copy is the minimum of the 2 vector lengths).
size_t copy_len =
(new_visuals.size() > visuals_.size()) ?
visuals_.size() : new_visuals.size();
for( size_t i = 0; i < copy_len; i++ )
{
int new_index = (messages_received_ - i) % new_visuals.size();
int old_index = (messages_received_ - i) % visuals_.size();
new_visuals[ new_index ] = visuals_[ old_index ];
visuals_[ old_index ] = NULL;
}
Delete any remaining old visuals
for( size_t i = 0; i < visuals_.size(); i++ ) {
delete visuals_[ i ];
}
We don’t need to create any new visuals here, they are created as needed when messages are received.
Put the new vector into the member variable version and let the old one go out of scope.
visuals_.swap( new_visuals );
}
void ImuDisplay::subscribe()
{
If we are not actually enabled, don’t do it.
if ( !isEnabled() )
{
return;
}
Try to subscribe to the current topic name (in topic_). Make sure to catch exceptions and set the status to a descriptive error message.
try
{
sub_.subscribe( update_nh_, topic_, 10 );
setStatus( rviz::status_levels::Ok, "Topic", "OK" );
}
catch( ros::Exception& e )
{
setStatus( rviz::status_levels::Error, "Topic",
std::string( "Error subscribing: " ) + e.what() );
}
}
void ImuDisplay::unsubscribe()
{
sub_.unsubscribe();
}
void ImuDisplay::onEnable()
{
subscribe();
}
void ImuDisplay::onDisable()
{
unsubscribe();
clear();
}
When the “Fixed Frame” changes, we need to update our tf::MessageFilter and erase existing visuals.
void ImuDisplay::fixedFrameChanged()
{
tf_filter_->setTargetFrame( fixed_frame_ );
clear();
}
This is our callback to handle an incoming message.
void ImuDisplay::incomingMessage( const sensor_msgs::Imu::ConstPtr& msg )
{
++messages_received_;
Each display can have multiple status lines. This one is called “Topic” and says how many messages have been received in this case.
std::stringstream ss;
ss << messages_received_ << " messages received";
setStatus( rviz::status_levels::Ok, "Topic", ss.str() );
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( !vis_manager_->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(), fixed_frame_.c_str() );
return;
}
We are keeping a circular buffer of visual pointers. This gets the next one, or creates and stores it if it was missing.
ImuVisual* visual = visuals_[ messages_received_ % history_length_ ];
if( visual == NULL )
{
visual = new ImuVisual( vis_manager_->getSceneManager(), scene_node_ );
visuals_[ messages_received_ % history_length_ ] = visual;
}
Now set or update the contents of the chosen visual.
visual->setMessage( msg );
visual->setFramePosition( position );
visual->setFrameOrientation( orientation );
visual->setColor( color_.r_, color_.g_, color_.b_, alpha_ );
}
Override rviz::Display’s reset() function to add a call to clear().
void ImuDisplay::reset()
{
Display::reset();
clear();
}
Override createProperties() to build and configure a Property object for each user-editable property. property_manager_, property_prefix_, and parent_category_ are all initialized before this is called.
void ImuDisplay::createProperties()
{
topic_property_ =
property_manager_->createProperty<rviz::ROSTopicStringProperty>( "Topic",
property_prefix_,
boost::bind( &ImuDisplay::getTopic, this ),
boost::bind( &ImuDisplay::setTopic, this, _1 ),
parent_category_,
this );
setPropertyHelpText( topic_property_, "sensor_msgs::Imu topic to subscribe to." );
rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
topic_prop->setMessageType( ros::message_traits::datatype<sensor_msgs::Imu>() );
color_property_ =
property_manager_->createProperty<rviz::ColorProperty>( "Color",
property_prefix_,
boost::bind( &ImuDisplay::getColor, this ),
boost::bind( &ImuDisplay::setColor, this, _1 ),
parent_category_,
this );
setPropertyHelpText( color_property_, "Color to draw the acceleration arrows." );
alpha_property_ =
property_manager_->createProperty<rviz::FloatProperty>( "Alpha",
property_prefix_,
boost::bind( &ImuDisplay::getAlpha, this ),
boost::bind( &ImuDisplay::setAlpha, this, _1 ),
parent_category_,
this );
setPropertyHelpText( alpha_property_, "0 is fully transparent, 1.0 is fully opaque." );
history_length_property_ =
property_manager_->createProperty<rviz::IntProperty>( "History Length",
property_prefix_,
boost::bind( &ImuDisplay::getHistoryLength, this ),
boost::bind( &ImuDisplay::setHistoryLength, this, _1 ),
parent_category_,
this );
setPropertyHelpText( history_length_property_, "Number of prior measurements to display." );
}
} // end namespace rviz_plugin_tutorials
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_DECLARE_CLASS( rviz_plugin_tutorials, Imu, rviz_plugin_tutorials::ImuDisplay, rviz::Display )
imu_visual.h¶
The full text of imu_visual.h is here: src/imu_visual.h
Declare the visual class for this display.
Each instance of ImuVisual represents the visualization of a single sensor_msgs::Imu 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 ImuVisual
{
public:
Constructor. Creates the visual stuff and puts it into the scene, but in an unconfigured state.
ImuVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node );
Destructor. Removes the visual stuff from the scene.
virtual ~ImuVisual();
Configure the visual to show the data in the message.
void setMessage( const sensor_msgs::Imu::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 ImuVisual 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 Imu message.
void setColor( float r, float g, float b, float a );
private:
The object implementing the actual arrow shape
rviz::Arrow* acceleration_arrow_;
A SceneNode whose pose is set to match the coordinate frame of the Imu 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_;
};
imu_visual.cpp¶
The full text of imu_visual.cpp is here: src/imu_visual.cpp
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 Imu’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.
acceleration_arrow_ = new rviz::Arrow( scene_manager_, frame_node_ );
}
ImuVisual::~ImuVisual()
{
Delete the arrow to make it disappear.
delete acceleration_arrow_;
Destroy the frame node since we don’t need it anymore.
scene_manager_->destroySceneNode( frame_node_ );
}
void ImuVisual::setMessage( const sensor_msgs::Imu::ConstPtr& msg )
{
const geometry_msgs::Vector3& a = msg->linear_acceleration;
Convert the geometry_msgs::Vector3 to an Ogre::Vector3.
Ogre::Vector3 acc( a.x, a.y, a.z );
Find the magnitude of the acceleration vector.
float length = acc.length();
Scale the arrow’s thickness in each dimension along with its length.
Ogre::Vector3 scale( length, length, length );
acceleration_arrow_->setScale( scale );
Set the orientation of the arrow to match the direction of the acceleration vector.
acceleration_arrow_->setDirection( acc );
}
Position and orientation are passed through to the SceneNode.
void ImuVisual::setFramePosition( const Ogre::Vector3& position )
{
frame_node_->setPosition( position );
}
void ImuVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
{
frame_node_->setOrientation( orientation );
}
Color is passed through to the Arrow object.
void ImuVisual::setColor( float r, float g, float b, float a )
{
acceleration_arrow_->setColor( r, g, b, a );
}
Building the Plugin¶
To build the plugin, just do the normal “rosmake” thing:
rosmake rviz_plugin_tutorials
Exporting the Plugin¶
For the plugin to be found and understood by other ROS packages (in this case, rviz), it needs a “plugin_description.xml” file. This file can be named anything you like, as it is specified in the plugin package’s “manifest.xml” file like so:
<export>
<rviz plugin="${prefix}/plugin_description.xml"/>
</export>
The contents of plugin_description.xml then look like this:
<library path="lib/librviz_plugin_tutorials">
<class name="rviz_plugin_tutorials/Teleop"
type="rviz_plugin_tutorials::TeleopPanel"
base_class_type="rviz::Panel">
<description>
A panel widget allowing simple diff-drive style robot base control.
</description>
</class>
<class name="rviz_plugin_tutorials/Imu"
type="rviz_plugin_tutorials::ImuDisplay"
base_class_type="rviz::Display">
<description>
Displays direction and scale of accelerations from sensor_msgs/Imu messages.
</description>
</class>
</library>
The first line says that the compiled library lives in lib/librviz_plugin_tutorials (the ”.so” ending is appended by pluginlib according to the OS). This path is relative to the top directory of the package:
<library path="lib/librviz_plugin_tutorials">
The next section is a class entry describing the TeleopPanel:
<class name="rviz_plugin_tutorials/Teleop"
type="rviz_plugin_tutorials::TeleopPanel"
base_class_type="rviz::Panel">
<description>
A panel widget allowing simple diff-drive style robot base control.
</description>
</class>
This specifies the name, type, base class, and description of the class. The name field must be a combination of the first two strings given to the PLUGINLIB_DECLARE_CLASS() macro in the source file. It must be the “package” name, a “/” slash, then the “display name” for the class.
The type entry must be the fully-qualified class name, including any namespace(s) it is inside.
The base_class_type is either rviz::Panel for a panel class, or rviz::Display for a display class.
The description subsection is a simple text description of the class, which is shown in the class-chooser dialog and in the Displays panel help area. This section can contain HTML, including hyperlinks, but the markup must be escaped to avoid being interpreted as XML markup. For example a link tag might look like: <a href="my-web-page.html">.
Trying It Out¶
Once your RViz plugin is compiled and exported, simply run rviz normally:
rosrun rviz rviz
and rviz will use pluginlib to find all the plugins exported to it.
Add an ImuDisplay by clicking the “Add” button at the bottom of the “Displays” panel (or by typing Control-N), then scrolling down through the available displays until you see “Imu” under your plugin package name (here it is “rviz_plugin_tutorials”).
If “Imu” is not in your list of Display Types, look through RViz’s console output for error messages relating to plugin loading. Some common problems are:
- not having a plugin_description.xml file,
- not exporting it in the manifest.xml file, or
- not properly referencing the library file (like librviz_plugin_tutorials.so) from plugin_description.xml.
Once you’ve added the Imu display to RViz, you just need to set the topic name of the display to a source of sensor_msgs/Imu messages.
If you don’t happen to have an IMU or other source of sensor_msgs/Imu messages, you can test the plugin with a Python script like this: scripts/send_test_msgs.py.
The script publishes on the “/test_imu” topic, so enter that.
The script publishes both Imu messages and a moving TF frame (“/base_link” relative to “/map”), so make sure your “Fixed Frame” is set to “/map”.
Finally, adjust the “History Length” parameter of the Imu display to 10 and you should see something like the picture at the top of this page.
Note: If you use this to visualize messages from an actual IMU, the arrows are going to be huge compared to most robots:
(Note the PR2 robot at the base of the purple arrow.) This is because the Imu acceleration units are meters per second squared, and gravity is 9.8 m/s^2, and we haven’t applied any scaling or gravity compensation to the acceleration vectors.
Next Steps¶
This ImuDisplay is not yet a terribly useful Display class. Extensions to make it more useful might be:
- Add a gravity-compensation option to the acceleration vector.
- Visualize more of the data in the Imu messages.
To add a gravity compensation option, you might take steps like these:
- Add a new bool gravity_compensation_on_ property to ImuDisplay to store whether the option is on or off.
- Add getter and setter functions for it.
- Add a new rviz::BoolPropertyWPtr to ImuDisplay to show the property in the property editor.
- Add a new setGravityCompensation(bool) function to ImuVisual to tell the visual whether to subtract out gravity or not.
- Compute the direction of gravity relative to the Imu frame orientation (as set in ImuVisual::setFrameOrientation()) and subtract it from the acceleration vector each time in ImuVisual::setMessage().
Since ImuVisual takes complete Imu messages as input, adding visualizations of more of the Imu data only needs modifications to ImuVisual. Imu data displays might look like:
- orientation: An rviz::Axes object at the Imu reference frame, turned to show the orientation.
- angular_velocity: Maybe a line to show the axis of rotation and a 3D arrow curving around it to show the speed of rotation?
- orientation_covariance: Maybe this is an ellipse at the end of each of the X, Y, and Z axes showing the orientation?
- linear_acceleration_covariance: Maybe this is an ellipsoid at the end of the acceleration arrow?
As all this might be visually cluttered, it may make sense to include boolean options to enable or disable some of them.
Conclusion¶
Remember, the plugin API for RViz is still preliminary. I think there are a number of ways the process of writing a plugin can be streamlined before the Groovy release. In the meantime, please try out this Fuerte API version and let us know what you think!