Rviz教程（九）：Plugins: New Tool Type

PlantFlagTool¶

Overview

This tutorial shows how to write a new tool for RViz.

In RViz, a tool is a class that determines how mouse events interact with the visualizer. In this example we describe PlantFlagTool which lets you place “flag” markers in the 3D scene.

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-hydro-visualization-tutorials

Here is an example of what the new PlantFlagTool can do:

The Plugin Code

The code for PlantFlagTool is in these files: src/plant_flag_tool.h, and src/plant_flag_tool.cpp.

plant_flag_tool.h

The full text of plant_flag_tool.h is here: src/plant_flag_tool.h

Here we declare our new subclass of rviz::Tool. Every tool which can be added to the tool bar is a subclass of rviz::Tool.

class PlantFlagTool: public rviz::Tool
{
Q_OBJECT
public:
  PlantFlagTool();
  ~PlantFlagTool();

  virtual void onInitialize();

  virtual void activate();
  virtual void deactivate();

  virtual int processMouseEvent( rviz::ViewportMouseEvent& event );

  virtual void load( const rviz::Config& config );
  virtual void save( rviz::Config config ) const;

private:
  void makeFlag( const Ogre::Vector3& position );

  std::vector<Ogre::SceneNode*> flag_nodes_;
  Ogre::SceneNode* moving_flag_node_;
  std::string flag_resource_;
  rviz::VectorProperty* current_flag_property_;
};

plant_flag_tool.cpp

The full text of plant_flag_tool.cpp is here: src/plant_flag_tool.cpp

Construction and destruction

The constructor must have no arguments, so we can’t give the constructor the parameters it needs to fully initialize.

Here we set the “shortcut_key_” member variable defined in the superclass to declare which key will activate the tool.

PlantFlagTool::PlantFlagTool()
  : moving_flag_node_( NULL )
  , current_flag_property_( NULL )
{
  shortcut_key_ = 'l';
}

The destructor destroys the Ogre scene nodes for the flags so they disappear from the 3D scene. The destructor for a Tool subclass is only called when the tool is removed from the toolbar with the “-” button.

PlantFlagTool::~PlantFlagTool()
{
  for( unsigned i = 0; i < flag_nodes_.size(); i++ )
  {
    scene_manager_->destroySceneNode( flag_nodes_[ i ]);
  }
}

onInitialize() is called by the superclass after scene_manager_ and context_ are set. It should be called only once per instantiation. This is where most one-time initialization work should be done. onInitialize() is called during initial instantiation of the tool object. At this point the tool has not been activated yet, so any scene objects created should be invisible or disconnected from the scene at this point.

In this case we load a mesh object with the shape and appearance of the flag, create an Ogre::SceneNode for the moving flag, and then set it invisible.

void PlantFlagTool::onInitialize()
{
  flag_resource_ = "package://rviz_plugin_tutorials/media/flag.dae";

  if( rviz::loadMeshFromResource( flag_resource_ ).isNull() )
  {
    ROS_ERROR( "PlantFlagTool: failed to load model resource '%s'.", flag_resource_.c_str() );
    return;
  }

  moving_flag_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  Ogre::Entity* entity = scene_manager_->createEntity( flag_resource_ );
  moving_flag_node_->attachObject( entity );
  moving_flag_node_->setVisible( false );
}

Activation and deactivation

activate() is called when the tool is started by the user, either by clicking on its button in the toolbar or by pressing its hotkey.

First we set the moving flag node to be visible, then we create an rviz::VectorProperty to show the user the position of the flag. Unlike rviz::Display, rviz::Tool is not a subclass of rviz::Property, so when we want to add a tool property we need to get the parent container with getPropertyContainer() and add it to that.

We wouldn’t have to set current_flag_property_ to be read-only, but if it were writable the flag should really change position when the user edits the property. This is a fine idea, and is possible, but is left as an exercise for the reader.

void PlantFlagTool::activate()
{
  if( moving_flag_node_ )
  {
    moving_flag_node_->setVisible( true );

    current_flag_property_ = new rviz::VectorProperty( "Flag " + QString::number( flag_nodes_.size() ));
    current_flag_property_->setReadOnly( true );
    getPropertyContainer()->addChild( current_flag_property_ );
  }
}

deactivate() is called when the tool is being turned off because another tool has been chosen.

We make the moving flag invisible, then delete the current flag property. Deleting a property also removes it from its parent property, so that doesn’t need to be done in a separate step. If we didn’t delete it here, it would stay in the list of flags when we switch to another tool.

void PlantFlagTool::deactivate()
{
  if( moving_flag_node_ )
  {
    moving_flag_node_->setVisible( false );
    delete current_flag_property_;
    current_flag_property_ = NULL;
  }
}

Handling mouse events

processMouseEvent() is sort of the main function of a Tool, because mouse interactions are the point of Tools.

We use the utility function rviz::getPointOnPlaneFromWindowXY() to see where on the ground plane the user’s mouse is pointing, then move the moving flag to that point and update the VectorProperty.

If this mouse event was a left button press, we want to save the current flag location. Therefore we make a new flag at the same place and drop the pointer to the VectorProperty. Dropping the pointer means when the tool is deactivated the VectorProperty won’t be deleted, which is what we want.

int PlantFlagTool::processMouseEvent( rviz::ViewportMouseEvent& event )
{
  if( !moving_flag_node_ )
  {
    return Render;
  }
  Ogre::Vector3 intersection;
  Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
  if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
                                         ground_plane,
                                         event.x, event.y, intersection ))
  {
    moving_flag_node_->setVisible( true );
    moving_flag_node_->setPosition( intersection );
    current_flag_property_->setVector( intersection );

    if( event.leftDown() )
    {
      makeFlag( intersection );
      current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
      return Render | Finished;
    }
  }
  else
  {
    moving_flag_node_->setVisible( false ); // If the mouse is not pointing at the ground plane, don't show the flag.
  }
  return Render;
}

This is a helper function to create a new flag in the Ogre scene and save its scene node in a list.

void PlantFlagTool::makeFlag( const Ogre::Vector3& position )
{
  Ogre::SceneNode* node = scene_manager_->getRootSceneNode()->createChildSceneNode();
  Ogre::Entity* entity = scene_manager_->createEntity( flag_resource_ );
  node->attachObject( entity );
  node->setVisible( true );
  node->setPosition( position );
  flag_nodes_.push_back( node );
}

Loading and saving the flags

Tools with a fixed set of Property objects representing adjustable parameters are typically just created in the tool’s constructor and added to the Property container (getPropertyContainer()). In that case, the Tool subclass does not need to override load() and save() because the default behavior is to read all the Properties in the container from the Config object.

Here however, we have a list of named flag positions of unknown length, so we need to implement save() and load() ourselves.

We first save the class ID to the config object so the rviz::ToolManager will know what to instantiate when the config file is read back in.

void PlantFlagTool::save( rviz::Config config ) const
{
  config.mapSetValue( "Class", getClassId() );

The top level of this tool’s Config is a map, but our flags should go in a list, since they may or may not have unique keys. Therefore we make a child of the map (flags_config) to store the list.

rviz::Config flags_config = config.mapMakeChild( "Flags" );

To read the positions and names of the flags, we loop over the the children of our Property container:

rviz::Property* container = getPropertyContainer();
int num_children = container->numChildren();
for( int i = 0; i < num_children; i++ )
{
  rviz::Property* position_prop = container->childAt( i );

For each Property, we create a new Config object representing a single flag and append it to the Config list.

rviz::Config flag_config = flags_config.listAppendNew();

Into the flag’s config we store its name:

flag_config.mapSetValue( "Name", position_prop->getName() );

... and its position.

    position_prop->save( flag_config );
  }
}

In a tool’s load() function, we don’t need to read its class because that has already been read and used to instantiate the object before this can have been called.

void PlantFlagTool::load( const rviz::Config& config )
{

Here we get the “Flags” sub-config from the tool config and loop over its entries:

rviz::Config flags_config = config.mapGetChild( "Flags" );
int num_flags = flags_config.listLength();
for( int i = 0; i < num_flags; i++ )
{
  rviz::Config flag_config = flags_config.listChildAt( i );

At this point each flag_config represents a single flag.

Here we provide a default name in case the name is not in the config file for some reason:

QString name = "Flag " + QString::number( i + 1 );

Then we use the convenience function mapGetString() to read the name from flag_config if it is there. (If no “Name” entry were present it would return false, but we don’t care about that because we have already set a default.)

flag_config.mapGetString( "Name", &name );

Given the name we can create an rviz::VectorProperty to display the position:

rviz::VectorProperty* prop = new rviz::VectorProperty( name );

Then we just tell the property to read its contents from the config, and we’ve read all the data.

prop->load( flag_config );

We finish each flag by marking it read-only (as discussed above), adding it to the property container, and finally making an actual visible flag object in the 3D scene at the correct position.

    prop->setReadOnly( true );
    getPropertyContainer()->addChild( prop );
    makeFlag( prop->getVector() );
  }
}

End of .cpp file

At the end of every plugin class implementation, we end the namespace and then tell pluginlib about the class. It is important to do this in global scope, outside our package’s namespace.

} // end namespace rviz_plugin_tutorials

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(rviz_plugin_tutorials::PlantFlagTool,rviz::Tool )

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>
    <message_type>sensor_msgs/Imu</message_type>
  </class>
  <class name="rviz_plugin_tutorials/PlantFlag"
         type="rviz_plugin_tutorials::PlantFlagTool"
         base_class_type="rviz::Tool">
    <description>
      Tool for planting flags on the ground plane in rviz.
    </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 “display name” is the name used for the class in the user interface.

The type entry must be the fully-qualified class name, including any namespace(s) it is inside.

The base_class_type is usually one of rviz::Panel, rviz::Display, rviz::Tool, or rviz::ViewController.

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">.

Display plugins can have multiple message_type tags, which are used by RViz when you add a Display by selecting it’s topic first.

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 a PlantFlag tool by clicking on the “+” button in the toolbar and selecting “PlantFlag” from the list under your plugin package name (here it is “rviz_plugin_tutorials”).

Once “PlantFlag” is in your toolbar, click it or press “l” (the shortcut key) to start planting flags. Open the “Tool Properties” panel to see the positions of the flags you have planted.

Currently the only way to remove the flags is to delete the PlantFlag tool, which you do by pressing the “-” (minus sign) button in the toolbar and selecting “PlantFlag”.

Next Steps

PlantFlag as shown here is not terribly useful yet. Some extensions to make it more useful might be:

• Add the ability to delete, re-position, and re-name existing flags.
• Publish ROS messages with the names and locations of the flags.

To modify existing flags, you might:

• Change processMouseEvent() to notice when the mouse is pointing near an existing flag.
• When it is:
  • make the flag highlight.
  • If the right button is pressed, show a context menu with delete and rename options.
  • If the left button is pressed, begin dragging the existing flag around.

Conclusion

There are many possibilities for new types of interaction with RViz. We look forward to seeing what you make.